TRABAJO CON vtkplotlib

[jurekalfredo]

https://user-images.githubusercontent.com/41833946/175405754-41931bb9-967e-4b51-a4cd-018808843b3c.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]

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