-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathVizSceneSource.py
142 lines (106 loc) · 4.98 KB
/
VizSceneSource.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
import pyqtgraph as pg
from pyqtgraph import opengl as gl
import numpy as np
def cube_vertex(a, b, c):
"""Returns a 8 x 3 numpy array of points describing the vertices of a rectangular prism, centered on the origin,
with x side length of a, y side length of b, and z side length of c."""
x = a / 2
y = b / 2
z = c / 2
vertices = np.array([[-x, -y, -z],
[x, -y, -z],
[-x, y, -z],
[x, y, -z],
[-x, -y, z],
[x, -y, z],
[-x, y, z],
[x, y, z]])
return vertices
def cube_vertex_to_mesh(v):
"""Takes a set of vertices as defined in cube_vertex and converts it to a numpy array suitable for use as a meshData
for GLMeshItem objects"""
mesh = np.array([[v[0], v[2], v[1]],
[v[1], v[2], v[3]],
[v[0], v[4], v[1]],
[v[1], v[4], v[5]],
[v[0], v[6], v[4]],
[v[0], v[2], v[6]],
[v[2], v[6], v[3]],
[v[3], v[6], v[7]],
[v[3], v[7], v[5]],
[v[1], v[3], v[5]],
[v[4], v[6], v[5]],
[v[5], v[6], v[7]]])
return mesh
class VizSceneObject:
"""A VizSceneObject is the parent class for an object that can be added to a viz scene and updated using some
parameter. For example, a robot arm is a VizSceneObject instance that has a mesh and can be updated by giving it
a set of joint values. A single link is also a VizSceneObject that returns a mesh based on a homogeneous transform.
Every VizSceneObject must define the following methods:
__init__(*args, **kwargs)
get_mesh(*args, **kwargs)
get_colors(*args, **kwargs)
The following methods are defined already:
get_GLMeshItem()
update(*args, **kwargs)
add(window)
additionally, for most objects it makes sense to have a function to redefine __init__ parameters (e.g. change color
or shape of different parts)
To give an example, we consider a VizFrame object. The VizFrame object is defined by a scale, colors, and label:
__init__(self, scale=1, colors=[[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 0]], label=None)
This function defines the points making up the mesh when the frame is at the origin. The get_GLMeshItem() method
returns a gl.GLMeshItem with the correct mesh and colors. The add function then adds this object to an existing
GLViewWidget.
The update function for a frame accepts a homogeneous transform and calls the get_mesh and get_colors methods based
on that information to update things:
def self.update(*args, **kwargs):
mesh = self.get_mesh(*args, **kwargs)
colors = self.get_colors(*args, **kwargs)
self.glItem.setMeshData(vertexes=mesh, vertexColors=colors)
VizSceneObjects should update a boolean property "fresh" that details if they have been changed, which the VizScene
can use to intelligently refresh only when things have changed.
"""
def __init__(self):
self.glItem = gl.GLMeshItem()
self.fresh = False
def get_mesh(self, *args, **kwargs):
raise NotImplementedError("VizSceneObject get_mesh function not implemented!")
def get_colors(self, *args, **kwargs):
raise NotImplementedError("VizSceneObject get_colors function not implemented!")
def get_GLMeshItem(self):
return self.glItem
def add(self, window):
window.addItem(self.glItem)
def update(self, *args, **kwargs):
mesh = self.get_mesh(*args, **kwargs)
colors = self.get_colors(*args, **kwargs)
self.glItem.setMeshData(vertexes=mesh,
vertexColors=colors)
def redefine(self):
pass
class VizSceneRigidBody(VizSceneObject):
"""
VizSceneRigidBody class describes an object to be added to a viz scene, which has a mesh that can be totally defined
by a homogeneous transform. In other words, a single rigid body with no moving or deformable parts whose update
method accepts a single 4x4 numpy array as input. This applies to frames, robot links, and revolute joints.
The get_mesh method for this class works by transforming a fixed array of vertex points by an accepted 4x4 matrix,
then generating a mesh based on these points.
The following methods are required by this class:
__init__()
get_points()
points_to_mesh()
get_colors()
The following methods are already defined by this class:
get_mesh()
"""
@staticmethod
def points_to_mesh(points):
raise NotImplementedError("VizSceneRigidBody points_to_mesh function not implemented!")
return np.eye(3)
def get_mesh(self, *args, **kwargs):
A = args[0]
R = A[0:3, 0:3]
p = A[0:3, 3]
points = self.get_points() @ R.T + p
mesh = self.points_to_mesh(points)
return mesh