TRABAJO CON vtkplotlib
Closed this issue · 1 comments
jurekalfredo commented
20220623_145431.mp4
estoy trabajando en n simulador de robot de 6gdl pero no logro que los movimientos sean fluidos
que es lo que estoy haciendo mal ????
import vtkplotlib as vpl
from stl.mesh import Mesh
import numpy as np
import math
global a
global b
global c
global d
global e
global f
def ZWOL_2_Pose(xyzrpw):
[x,y,z,r,p,w] = xyzrpw
a = r*np.pi/180.0
b = p*np.pi/180.0
c = w*np.pi/180.0
ca = np.cos(a)
sa = np.sin(a)
cb = np.cos(b)
sb = np.sin(b)
cc = np.cos(c)
sc = np.sin(c)
return np.array([[cb*ca, ca*sc*sb - cc*sa, sc*sa + cc*ca*sb, x],[cb*sa, cc*ca + sc*sb*sa, cc*sb*sa - ca*sc, y],[-sb, cb*sc, cc*cb, z],[0.0,0.0,0.0,1.0]])
mesh = Mesh.from_file("base.stl")
mesh2 = Mesh.from_file(r"J1_.stl")
mesh3 = Mesh.from_file(r"J2_.stl")
mesh4 = Mesh.from_file(r"J3_.stl")
mesh5 = Mesh.from_file(r"J4_.stl")
mesh6 = Mesh.from_file(r"J5_.stl")
mesh7 = Mesh.from_file(r"J6_.stl")
TITA = 0,-40,30,-50,30,0
pos = (0,0,0,TITA[0],0,0)
pos = ZWOL_2_Pose(pos)
mesh2.transform(pos)
pos1 = (0,0,376.21,0,TITA[1],0)
pos1 = ZWOL_2_Pose(pos1)
pos2 = np.dot(pos,pos1)
mesh3.transform(pos2)
pos3 = (0,0,400,0,TITA[2],0)
pos3 = ZWOL_2_Pose(pos3)
pos3 = np.dot(pos2,pos3)
mesh4.transform(pos3)
pos5 = (218,0,50.78,0,0,TITA[3])
pos5 = ZWOL_2_Pose(pos5)
pos5 = np.dot(pos3,pos5)
mesh5.transform(pos5)
pos6 = (187,0,0,0,TITA[4],0)
pos6 = ZWOL_2_Pose(pos6)
pos6 = np.dot(pos5,pos6)
mesh6.transform(pos6)
pos7 = (89.55,0,0,TITA[5],0,0)
pos7 = ZWOL_2_Pose(pos7)
pos7 = np.dot(pos6,pos7)
mesh7.transform(pos7)
vpl.mesh_plot(mesh, color="lightblue")
a=vpl.mesh_plot(mesh2, color="lightblue")
b=vpl.mesh_plot(mesh3, color="lightblue")
c=vpl.mesh_plot(mesh4, color="lightblue")
d=vpl.mesh_plot(mesh5, color="lightblue")
e=vpl.mesh_plot(mesh6, color="lightblue")
f=vpl.mesh_plot(mesh7, color="lightblue")
figure = vpl.gcf()
figure.update()
figure = vpl.gcf()
figure.update()
figure.camera.Yaw(45)
figure.camera.Pitch(35)
figure.camera.Roll(-45)
figure.reset_camera()
figure.update()
#time.sleep(2)
def inicio():
global a
global b
global c
global d
global e
global f
def imprimo(ss):
global A
global B
global C
global D
global E
global F
figure = vpl.gcf()
figure.update()
try:
figure.remove_plot(A)
figure.remove_plot(B)
figure.remove_plot(C)
figure.remove_plot(D)
figure.remove_plot(E)
figure.remove_plot(F)
except:
pass
#print('remuevo')
figure.remove_plot(a)
figure.remove_plot(b)
figure.remove_plot(c)
figure.remove_plot(d)
figure.remove_plot(e)
figure.remove_plot(f)
W = vpl.mesh_plot(mesh, color="lightblue")
A = vpl.mesh_plot(ss[0], color="lightblue")
B = vpl.mesh_plot(ss[1], color="lightblue")
C = vpl.mesh_plot(ss[2], color="lightblue")
D = vpl.mesh_plot(ss[3], color="lightblue")
E = vpl.mesh_plot(ss[4], color="lightblue")
F = vpl.mesh_plot(ss[5], color="lightblue")#vpl.mesh_plot(ss[5], color="lightblue")
def Robot(TITA):
mesh = Mesh.from_file("base.stl")
mesh2 = Mesh.from_file(r"J1_.stl")
mesh3 = Mesh.from_file(r"J2_.stl")
mesh4 = Mesh.from_file(r"J3_.stl")
mesh5 = Mesh.from_file(r"J4_.stl")
mesh6 = Mesh.from_file(r"J5_.stl")
mesh7 = Mesh.from_file(r"J6_.stl")
pos = (0,0,0,TITA[0],0,0)
pos = ZWOL_2_Pose(pos)
mesh2.transform(pos)
pos1 = (0,0,376.21,0,TITA[1],0)
pos1 = ZWOL_2_Pose(pos1)
pos2 = np.dot(pos,pos1)
mesh3.transform(pos2)
pos3 = (0,0,400,0,TITA[2],0)
pos3 = ZWOL_2_Pose(pos3)
pos3 = np.dot(pos2,pos3)
mesh4.transform(pos3)
pos5 = (218,0,50.78,0,0,TITA[3])
pos5 = ZWOL_2_Pose(pos5)
pos5 = np.dot(pos3,pos5)
mesh5.transform(pos5)
pos6 = (187,0,0,0,TITA[4],0)
pos6 = ZWOL_2_Pose(pos6)
pos6 = np.dot(pos5,pos6)
mesh6.transform(pos6)
pos7 = (89.55,0,0,TITA[5],0,0)
pos7 = ZWOL_2_Pose(pos7)
pos7 = np.dot(pos6,pos7)
mesh7.transform(pos7)
Angulos = mesh2,mesh3,mesh4,mesh5,mesh6,mesh7
return Angulos
TITA = 0,-40,30,-50,30,0
#i=0
i = 0
while i <(10):
i +=0.5)
TITA = i,-40,30,-50,30,0
ss=Robot(TITA)
imprimo(ss)
for i in range(10):
#i=i+1
TITA = 10,-40+i,30,-50,30,0
ss=Robot(TITA)
imprimo(ss)
for i in range(10):
#i=i+1
TITA = 10,-30,30-i,-50,30,0
ss=Robot(TITA)
imprimo(ss)
for i in range(50+1):
#i=i+1
TITA = 10,-30,20,-50+i,30,0
ss=Robot(TITA)
imprimo(ss)
inicio()
# Show the figure
vpl.show()bwoodsend commented
You just need to make your code more efficient by reusing things. Every iteration, you're deleting all the old plots, rereading all meshes from file, then re-plotting them. Of course the frame rate is low!
Try instead setting the transform to the plot instead of the mesh. Example below. The important part is the line mesh.actor.SetUserMatrix(transform_to_vtk(pos)).
import vtkplotlib as vpl
from stl.mesh import Mesh
import numpy as np
from vtkmodules.vtkCommonMath import vtkMatrix4x4
def transform_to_vtk(m):
out = vtkMatrix4x4()
for i in range(4):
for j in range(4):
out.SetElement(i, j, m[j, i])
return out
mesh = vpl.mesh_plot(Mesh.from_file(vpl.data.get_rabbit_stl()))
vpl.show(block=False)
vpl.gcf().camera.Zoom(.5)
pos = np.eye(4)
while 1:
pos[:3, :3] = Mesh.rotation_matrix([0, 0, -1], .1) @ pos[:3, :3]
mesh.actor.SetUserMatrix(transform_to_vtk(pos))
vpl.gcf().update()
vpl.show()