def controller_state_update(self):
self.curve_roll.setData(self.imu_angle_data[0])
self.curve_pitch.setData(self.imu_angle_data[1])
self.curve_yaw.setData(self.imu_angle_data[2])
self.curve_roll_v.setData(self.imu_angle_v_data[0])
self.curve_pitch_v.setData(self.imu_angle_v_data[1])
self.curve_yaw_v.setData(self.imu_angle_v_data[2])
self.t_roll_angle.setText('roll: %0.1f' % (self.imu_angle_data[0][-1]))
self.t_pitch_angle.setText('pitch: %0.1f' %
(self.imu_angle_data[1][-1]))
self.t_yaw_angle.setText('yaw: %0.1f' % (self.imu_angle_data[2][-1]))
self.t_roll_angle_v.setText('roll: %0.1f' %
(self.imu_angle_v_data[0][-1]))
self.t_pitch_angle_v.setText('pitch: %0.1f' %
(self.imu_angle_v_data[1][-1]))
self.t_yaw_angle_v.setText('yaw: %0.1f' %
(self.imu_angle_v_data[2][-1]))
self.cube_mat = self.cube_quat.toRotationMatrix()
self.cube_mat_data = self.cube_mat.data()
self.cube_transform = pg.Transform3D(
self.cube_mat_data[0], self.cube_mat_data[1],
self.cube_mat_data[2], 0, self.cube_mat_data[3],
self.cube_mat_data[4], self.cube_mat_data[5], 0,
self.cube_mat_data[6], self.cube_mat_data[7],
self.cube_mat_data[8], 0, 0, 0, 0, 1)
self.cube.setTransform(self.cube_transform)
def __init__(self, x=0, y=0, z=0, scale=0.1, widget=None):
"""
If there is no widget given a new window is created and its id is accessible via self.widget.
:param x: optional, float
:param y: optional, float
:param z: optional, float
:param widget: optional, gl.GLViewWidget
:return: None
"""
if not widget:
self.app = QtGui.QApplication([])
self.widget = gl.GLViewWidget()
self.widget.opts['distance'] = 40
self.widget.show()
else:
self.app = False
self.widget = widget
self.scale = scale
self.x = x
self.y = y
self.z = z
self.rot0 = 0
self.rot1 = 0
self.rot2 = 0
self.axis = gl.GLAxisItem()
tr = pg.Transform3D()
tr.scale(self.scale)
tr.rotate(self.rot0, 1, 0, 0)
tr.rotate(self.rot1, 0, 1, 0)
tr.rotate(self.rot2, 0, 0, 1)
self.axis.setTransform(tr)
self.axis.translate(self.x, self.y, self.z)
self.widget.addItem(self.axis)
def update():
dataLoad()
global data
global pos
#print(data[-1])
curve1.setData(data[0])
curve2.setData(data[1])
temp = pg.Transform3D(tr)
temp.rotate(data[0][-1], 0, 1, 0)
box.setTransform(temp)
if sys.stdin.buffer.peek() != '':
a = input()
s.write(a)
def update(self, x=None, y=None, z=None, scale=None):
if x:
self.x = x
if y:
self.y = y
if z:
self.z = z
if scale:
self.scale = scale
tr = pg.Transform3D()
tr.scale(self.scale)
self.grid.setTransform(tr)
self.grid.translate(self.x, self.y, self.z)
def _AffineTransform_to_pg(self, tr):
import pyqtgraph
if tr.dims == (2, 2):
m = tr.matrix
o = tr.offset
ptr = pyqtgraph.QtGui.QTransform(m[0,0], m[1,0], 0.0, m[0,1], m[1,1], 0.0, o[0], o[1], 1.0)
return ptr
elif tr.dims == (3, 3):
m = np.eye(4)
m[:3, :3] = tr.matrix
m[:3, 3] = tr.offset
ptr = pyqtgraph.Transform3D(m)
return ptr
else:
raise TypeError("Converting AffineTransform of dimension %r to pyqtgraph is not supported." % tr.dims)
def updateMeshTransform(self, q, t=[0, 0, 1]):
v = numpy.array(q[1:])
vnorm = numpy.linalg.norm(v)
v = v * 1 / vnorm
angle = 2 * math.atan2(vnorm, q[0])
# print('3d', angle, v)
tr = pg.Transform3D()
tr.translate(*t)
tr.rotate(rad2deg(angle), *v)
tr.rotate(90, 1, 0, 0)
# tr.translate(0, 0, -0.5)
tr.scale(0.015, 0.015, 0.015)
self.mesh_item.setTransform(tr)
def update(self, x=None, y=None, z=None, rot0=None, rot1=None, rot2=None):
if x:
self.x = x
if y:
self.y = y
if z:
self.z = z
if rot0:
self.rot0 = rot0
if rot1:
self.rot1 = rot1
if rot2:
self.rot2 = rot2
tr = pg.Transform3D()
tr.scale(self.scale)
tr.rotate(self.rot0, 1, 0, 0)
tr.rotate(self.rot1, 0, 1, 0)
tr.rotate(self.rot2, 0, 0, 1)
self.axis.setTransform(tr)
self.axis.translate(self.x, self.y, self.z)
def testMatrix():
"""
SRTTransform3D => Transform3D => SRTTransform3D
"""
tr = pg.SRTTransform3D()
tr.setRotate(45, (0, 0, 1))
tr.setScale(0.2, 0.4, 1)
tr.setTranslate(10, 20, 40)
assert tr.getRotation() == (45, QtGui.QVector3D(0, 0, 1))
assert tr.getScale() == QtGui.QVector3D(0.2, 0.4, 1)
assert tr.getTranslation() == QtGui.QVector3D(10, 20, 40)
tr2 = pg.Transform3D(tr)
assert np.all(tr.matrix() == tr2.matrix())
# This is the most important test:
# The transition from Transform3D to SRTTransform3D is a tricky one.
tr3 = pg.SRTTransform3D(tr2)
assert_array_almost_equal(tr.matrix(), tr3.matrix())
assert_almost_equal(tr3.getRotation()[0], tr.getRotation()[0])
assert_array_almost_equal(tr3.getRotation()[1], tr.getRotation()[1])
assert_array_almost_equal(tr3.getScale(), tr.getScale())
assert_array_almost_equal(tr3.getTranslation(), tr.getTranslation())
def test_pyqtgraph_conversion(self):
if not HAVE_PYQTGRAPH:
self.skipTest("pyqtgraph could not be imported")
pts2 = np.random.normal(size=(100, 2))
qtr1 = pyqtgraph.QtGui.QTransform()
qtr1.scale(1.1, -9)
qtr1.translate(3, -30)
qtr1.rotate(32)
t_atr = linear.STTransform.convert_from(qtr1, 'pyqtgraph')
qtr2 = t_atr.convert_to('pyqtgraph')
assert np.allclose(
pyqtgraph.transformCoordinates(qtr1, pts2, transpose=True),
t_atr.map(pts2))
assert np.allclose(
pyqtgraph.transformCoordinates(qtr2, pts2, transpose=True),
t_atr.map(pts2))
pts3 = np.random.normal(size=(100, 3))
qtr1 = pyqtgraph.Transform3D()
qtr1.scale(1.1, -9, 1e4)
qtr1.translate(3, -30, 0)
qtr1.rotate(32, 0, 1, 2)
t_atr = linear.AffineTransform.convert_from(qtr1, 'pyqtgraph')
print(t_atr)
qtr2 = t_atr.convert_to('pyqtgraph')
assert np.allclose(
pyqtgraph.transformCoordinates(qtr1, pts3, transpose=True),
t_atr.map(pts3))
assert np.allclose(
pyqtgraph.transformCoordinates(qtr2, pts3, transpose=True),
t_atr.map(pts3))
def make_volume_data(self, resolution=1.0, max_size=500e6):
"""
Using the current state of vertexes, edges, generates a scalar field
useful for building isosurface or volumetric renderings.
Parameters
----------
resolution: float, default=1.0 microns
width (um) of a single voxel in the scalar field.
max_size: int
maximum allowed scalar field size (bytes).
Returns
-------
* 3D scalar field indicating distance from nearest membrane,
* 3D field indicating section IDs of nearest membrane,
* QTransform that maps from 3D array indexes to original vertex
coordinates.
"""
vertexes, lines = self.get_geometry()
maxdia = vertexes['dia'].max() # maximum diameter (defines shape of kernel)
kernel_size = int(maxdia/resolution) + 3 # width of kernel
# read vertex data
pos = vertexes['pos']
d = vertexes['dia']
sec_id = vertexes['sec_index']
# decide on dimensions of scalar field
mx = pos.max(axis=0)
mn = pos.min(axis=0)
diff = mx - mn
shape = tuple((diff / resolution + kernel_size).astype(int))
# prepare blank scalar field for drawing
size = np.dtype(np.float32).itemsize * shape[0] * shape[1] * shape[2]
if size > max_size:
raise Exception("Scalar field would be larger than max_size (%dMB > %dMB), resolution is%f" % (size/1e6, max_size/1e6, resolution))
scfield = np.zeros(shape, dtype=np.float32)
scfield[:] = -1000
# array for holding IDs of sections that contribute to each area
idfield = np.empty(shape, dtype=int)
idfield[:] = -1
# map vertex locations to voxels
vox_pos = pos.copy()
vox_pos -= mn.reshape((1,3))
vox_pos *= 1./resolution
# Define kernel used to draw scalar field along dendrites
def cone(i,j,k):
# value decreases linearly with distance from center of kernel.
w = kernel_size / 2
return w - ((i-w)**2 + (j-w)**2 + (k-w)**2)**0.5
kernel = resolution * np.fromfunction(cone, (kernel_size,)*3)
kernel -= kernel.max()
def array_intersection(arr1, arr2, pos):
"""
Return slices used to access the overlapping area between two
arrays that are offset such that the origin of *arr2* is a *pos*
relative to *arr1*.
"""
s1 = [0]*3
s2 = [0]*3
t1 = [0]*3
t2 = [0]*3
pos = map(int, pos)
for axis in range(3):
s1[axis] = max(0, -pos[axis])
s2[axis] = min(arr2.shape[axis], arr1.shape[axis]-pos[axis])
t1[axis] = max(0, pos[axis])
t2[axis] = min(arr1.shape[axis], pos[axis]+arr2.shape[axis])
slice1 = (slice(t1[0],t2[0]), slice(t1[1],t2[1]), slice(t1[2],t2[2]))
slice2 = (slice(s1[0],s2[0]), slice(s1[1],s2[1]), slice(s1[2],s2[2]))
return slice1, slice2
# Draw lines into volume using *kernel* as the brush
vox_pos[:,0] = np.clip(vox_pos[:,0], 0, scfield.shape[0]-1)
vox_pos[:,1] = np.clip(vox_pos[:,1], 0, scfield.shape[1]-1)
vox_pos[:,2] = np.clip(vox_pos[:,2], 0, scfield.shape[2]-1)
for c in range(lines.shape[0]):
i = lines[c, 0]
j = lines[c, 1]
p1 = vox_pos[i].copy()
p2 = vox_pos[j].copy()
diff = p2-p1
axis = np.argmax(np.abs(diff))
dia = d[i]
nvoxels = abs(int(diff[axis]))+1
for k in range(nvoxels):
kern = kernel + (dia/2.0)
sl1, sl2 = array_intersection(scfield, kern, p1) # find the overlapping area between the field and the kernel
idfield[sl1] = np.where(scfield[sl1] > kern[sl2], idfield[sl1], sec_id[i])
scfield[sl1] = np.where(scfield[sl1] > kern[sl2], scfield[sl1], kern[sl2])
dia += (d[j]-d[i]) / nvoxels
p1 += diff / nvoxels
# return transform relating volume data to original vertex data
transform = pg.Transform3D()
w = resolution * kernel_size / 2 # offset introduced due to kernel
transform.translate(*(mn-w))
transform.scale(resolution, resolution, resolution)
transform.translate(1, 1, 1)
return scfield, idfield, transform
def __init__(self):
self.width = 1000
self.high = 1000
self.console_high = 150
self.plot_high = self.high - self.console_high
self.console_size = (self.width, self.console_high)
self.plot_size = (self.width, self.plot_high)
self.app = QtGui.QApplication([])
self.win = QtGui.QMainWindow()
self.area = DockArea()
self.win.setCentralWidget(self.area)
self.win.resize(self.width, self.high)
self.win.setWindowTitle('Gimbal Monitor')
self.timer_dt = 50
pg.setConfigOptions(antialias=True)
pg.setConfigOption('background', [0, 0, 0])
pg.setConfigOption('foreground', [255, 255, 255, 100])
# Dock
self.d_console = self.dock4console('Console')
self.d_controller_state = self.dock4plot('Controller State')
self.d_motors_state = self.dock4plot('Motors State')
self.d_controller_config = self.dock4plot('Controller Config')
self.d_motors_config = self.dock4plot('Motor Config')
self.d_imu_attitude = Dock('IMU Attitude',
size=(1000, self.plot_high),
closable=True)
#self.d_imu_attitude.show()
self.area.addDock(self.d_console, 'bottom')
self.area.addDock(self.d_controller_state, 'top')
self.area.addDock(self.d_motors_state, 'top')
self.area.addDock(self.d_controller_config, 'top')
self.area.addDock(self.d_motors_config, 'top')
self.area.addDock(self.d_imu_attitude, 'top')
self.area.moveDock(self.d_motors_state, 'above',
self.d_controller_state)
self.area.moveDock(self.d_controller_config, 'above',
self.d_motors_state)
self.area.moveDock(self.d_motors_config, 'above',
self.d_controller_config)
self.area.moveDock(self.d_imu_attitude, 'right',
self.d_controller_state)
# Controller State Layout
self.pen_width = 2
self.fill_beta = 70
self.pen_red = pg.mkPen('F00', width=self.pen_width)
self.pen_green = pg.mkPen('0F0', width=self.pen_width)
self.pen_blue = pg.mkPen('0AF', width=self.pen_width)
self.fill_red = [255, 0, 0, self.fill_beta]
self.fill_green = [0, 255, 0, self.fill_beta]
self.fill_blue = [0, 200, 255, self.fill_beta]
self.imu_angle_time_len = 200
self.imu_angle_data = np.zeros((3, self.imu_angle_time_len))
self.imu_angle_v_data = np.zeros((3, self.imu_angle_time_len))
self.w_controller_state = pg.LayoutWidget()
self.p_attitude = pg.PlotWidget(title="Camera Attitude")
self.p_angle_v = pg.PlotWidget(title="Angle Velocity")
self.p_attitude.showGrid(x=True, y=True)
self.p_angle_v.showGrid(x=True, y=True)
self.curve_roll = self.p_attitude.plot(self.imu_angle_data[0],
pen=self.pen_red,
fillLevel=0,
brush=self.fill_red)
self.curve_pitch = self.p_attitude.plot(self.imu_angle_data[1],
pen=self.pen_green,
fillLevel=0,
brush=self.fill_green)
self.curve_yaw = self.p_attitude.plot(self.imu_angle_data[2],
pen=self.pen_blue,
fillLevel=0,
brush=self.fill_blue)
self.curve_roll_v = self.p_angle_v.plot(self.imu_angle_v_data[0],
pen=self.pen_red,
fillLevel=0,
brush=self.fill_red)
self.curve_pitch_v = self.p_angle_v.plot(self.imu_angle_v_data[1],
pen=self.pen_green,
fillLevel=0,
brush=self.fill_green)
self.curve_yaw_v = self.p_angle_v.plot(self.imu_angle_v_data[2],
pen=self.pen_blue,
fillLevel=0,
brush=self.fill_blue)
self.time_remain = 6
self.angle_max = 100
self.angle_v_max = 1000
self.text_interval = self.angle_max * 0.12
self.text_interval_v = self.angle_v_max * 0.12
self.angle_text_max_y = self.angle_max * 1.12
self.angle_v_text_max_y = self.angle_v_max * 1.12
self.p_attitude.setXRange(0 - self.time_remain,
self.imu_angle_time_len + self.time_remain)
self.p_attitude.setYRange(-100, 100)
self.p_angle_v.setXRange(0 - self.time_remain,
self.imu_angle_time_len + self.time_remain)
self.p_angle_v.setYRange(-1000, 1000)
self.t_roll_angle = pg.TextItem()
self.t_roll_angle.setText('roll: %0.1f' % (0), [255, 0, 0, 200])
self.t_roll_angle.setPos(0, self.angle_text_max_y)
self.p_attitude.addItem(self.t_roll_angle)
self.t_pitch_angle = pg.TextItem()
self.t_pitch_angle.setText('pitch: %0.1f' % (0), [0, 255, 0, 200])
self.t_pitch_angle.setPos(0,
self.angle_text_max_y - self.text_interval)
self.p_attitude.addItem(self.t_pitch_angle)
self.t_yaw_angle = pg.TextItem()
self.t_yaw_angle.setText('yaw: %0.1f' % (0), [0, 200, 255, 230])
self.t_yaw_angle.setPos(0,
self.angle_text_max_y - 2 * self.text_interval)
self.p_attitude.addItem(self.t_yaw_angle)
self.t_roll_angle_v = pg.TextItem()
self.t_roll_angle_v.setText('roll: %0.1f' % (0), [255, 0, 0, 200])
self.t_roll_angle_v.setPos(0, self.angle_v_text_max_y)
self.p_angle_v.addItem(self.t_roll_angle_v)
self.t_pitch_angle_v = pg.TextItem()
self.t_pitch_angle_v.setText('pitch: %0.1f' % (0), [0, 255, 0, 200])
self.t_pitch_angle_v.setPos(
0, self.angle_v_text_max_y - self.text_interval_v)
self.p_angle_v.addItem(self.t_pitch_angle_v)
self.t_yaw_angle_v = pg.TextItem()
self.t_yaw_angle_v.setText('yaw: %0.1f' % (0), [0, 200, 255, 230])
self.t_yaw_angle_v.setPos(
0, self.angle_v_text_max_y - 2 * self.text_interval_v)
self.p_angle_v.addItem(self.t_yaw_angle_v)
self.w_controller_state.addWidget(self.p_attitude, row=0, col=0)
self.w_controller_state.addWidget(self.p_angle_v, row=1, col=0)
self.d_controller_state.addWidget(self.w_controller_state,
row=0,
col=1)
self.timer_controller_state = QtCore.QTimer()
self.timer_controller_state.timeout.connect(
self.controller_state_update)
# Motors State Layout
self.motors_state_time_len = 200
self.ntc_tempre_data = np.zeros((3, self.motors_state_time_len))
self.input_current_data = np.zeros((3, self.motors_state_time_len))
self.motor_current_data = np.zeros((3, self.motors_state_time_len))
self.input_v_data = np.zeros((3, self.motors_state_time_len))
self.duty_cycle_now_data = np.zeros((3, self.motors_state_time_len))
self.rpm_data = np.zeros((3, self.motors_state_time_len))
self.tacho_data = np.zeros((3, self.motors_state_time_len))
self.tacho_abs_data = np.zeros((3, self.motors_state_time_len))
self.w_motors_state = pg.LayoutWidget()
self.p_ntc_tempre = pg.PlotWidget()
self.p_current = pg.PlotWidget()
self.p_input_v = pg.PlotWidget()
self.p_duty_cycle_now = pg.PlotWidget()
self.p_rpm = pg.PlotWidget()
self.p_tacho = pg.PlotWidget()
self.p_tach_abs = pg.PlotWidget()
self.p_ntc_tempre.showGrid(True)
self.p_current.showGrid(True)
self.p_input_v.showGrid(True)
self.p_duty_cycle_now.showGrid(True)
self.p_rpm.showGrid(True)
self.p_tacho.showGrid(True)
self.p_tach_abs.showGrid(True)
self.curve_ntc_tempre = self.p_ntc_tempre.plot(self.ntc_tempre_data[0],
pen=self.pen_blue,
fillLevel=0,
brush=self.fill_blue)
self.curve_input_current = self.p_current.plot(
self.input_current_data[0], pen=self.pen_)
# IMU Attitude OpenGL Layout
self.vr_layout = pg.LayoutWidget()
self.w_vr = gl.GLViewWidget()
glEnable(GL_LINE_SMOOTH)
glShadeModel(GL_SMOOTH)
glEnable(GL_DEPTH_TEST)
glEnable(GL_CULL_FACE)
glCullFace(GL_BACK)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC1_ALPHA)
self.light()
glMatrixMode(GL_PROJECTION)
glMatrixMode(GL_MODELVIEW)
gluPerspective(40., 1., 1., 40.)
self.w_vr.opts['distance'] = 40
self.w_vr.opts['azimuth'] = -10
print(self.w_vr.opts)
self.w_vr.setWindowTitle('IMU Attitude')
#self.w_vr.setCameraPosition(pos=[0, 0, 50],)
self.w_vr.show()
self.vr_ground = gl.GLGridItem()
self.vr_ground.scale(10, 10, 1)
self.w_vr.addItem(self.vr_ground)
self.cube_width = 20
self.cube_high = self.cube_width * (1 - 0.618)
self.verts = np.array([
[self.cube_width / 2, self.cube_width / 2, self.cube_high / 2],
[-self.cube_width / 2, self.cube_width / 2, self.cube_high / 2],
[-self.cube_width / 2, -self.cube_width / 2, self.cube_high / 2],
[self.cube_width / 2, -self.cube_width / 2, self.cube_high / 2],
[self.cube_width / 2, self.cube_width / 2, -self.cube_high / 2],
[-self.cube_width / 2, self.cube_width / 2, -self.cube_high / 2],
[-self.cube_width / 2, -self.cube_width / 2, -self.cube_high / 2],
[self.cube_width / 2, -self.cube_width / 2, -self.cube_high / 2],
])
self.face = np.array([
[0, 1, 2], # up
[0, 2, 3],
[4, 6, 5], # down
[4, 7, 6],
[0, 3, 7], # front
[0, 7, 4],
[1, 6, 2], # back
[1, 5, 6],
[0, 4, 5], # left
[0, 5, 1],
[2, 6, 7], # right
[2, 7, 3],
])
self.alpha = 1
self.c_red = [1, 0, 0, self.alpha]
self.c_greed = [0, 1, 0, self.alpha]
self.c_blue = [0, 0, 1, self.alpha]
self.gray_scale = 0.8
self.c_gray = [
self.gray_scale, self.gray_scale, self.gray_scale, self.alpha
]
self.colors = np.array([
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
self.c_gray,
])
self.cube = gl.GLMeshItem(vertexes=self.verts,
faces=self.face,
faceColors=self.colors,
shader='shaded',
drawEdges=True,
edgeColor=(1, 1, 1, 1),
smooth=False)
self.cube.translate(0, 0, 3)
self.cube_quat = QQuaternion(1.0, 0.0, 0.0, 0.0)
self.cube_mat = self.cube_quat.toRotationMatrix()
self.cube_mat_data = self.cube_mat.data()
self.cube_transform = pg.Transform3D(
self.cube_mat_data[0], self.cube_mat_data[1],
self.cube_mat_data[2], 0, self.cube_mat_data[3],
self.cube_mat_data[4], self.cube_mat_data[5], 0,
self.cube_mat_data[6], self.cube_mat_data[7],
self.cube_mat_data[8], 0, 0, 0, 0, 1)
self.cube.setTransform(self.cube_transform)
self.w_vr.addItem(self.cube)
self.d_imu_attitude.addWidget(self.w_vr)
