def arcball(x, y, w, h):
r = (w + h) / 2.0
x = -(2.0 * x - w) / r
y = (2.0 * y - h) / r
h = math.hypot(x, y)
if h > 1:
return Quaternion(0, x / h, y / h, 0)
else:
return Quaternion(0, x, y, math.sqrt(1 - h**2))
def sample_quat(rx, ry, rz, degrees=False):
""" Classmethod to create a quaternion given the euler angles.
"""
if degrees:
rx, ry, rz = np.radians([rx, ry, rz])
# Obtain quaternions
qx = Quaternion(np.cos(rx / 2), np.sin(rx / 2), 0, 0)
qy = Quaternion(np.cos(ry / 2), 0, np.sin(ry / 2), 0)
qz = Quaternion(np.cos(rz / 2), 0, 0, np.sin(rz / 2))
# Almost done
return qx * qy * qz
def on_mouse_move(self, event):
if event.button == 1 and event.last_event is not None:
x0, y0 = event.last_event.pos
x1, y1 = event.pos
w, h = self.size
self.quaternion = (self.quaternion *
Quaternion(*_arcball(x0, y0, w, h)) *
Quaternion(*_arcball(x1, y1, w, h)))
self.cube.transform.matrix = self.quaternion.get_matrix()
self.cube.transform.scale((100, 100, 0.001))
self.cube.transform.translate((200, 200))
self.update()
def on_mouse_move(self, event):
if event.button == 1 and event.last_event is not None:
x0, y0 = event.last_event.pos
x1, y1 = event.pos
w, h = self.size
self.quaternion = (self.quaternion *
Quaternion(*_arcball(x0, y0, w, h)) *
Quaternion(*_arcball(x1, y1, w, h)))
self.rotMat = self.quaternion.get_matrix()
self.view = np.dot(self.rotMat, self.transMat)
for k, prog in self.GL_progs.items():
prog['u_view'] = self.view
prog['u_model'] = self.model
self.update()
def __init__(self, sensor_type="OrbbecAstra", sensor_scale=1.0):
app.Canvas.__init__(self, keys='interactive', size=(1280, 1024))
self.title = 'RGBDMultiView'
self.quaternion = Quaternion() # for mouse movement
self.cam_params = GetCameraParameters(sensor_type, sensor_scale)
self.fov = 30
ps = self.pixel_scale
self.playbackSpeed = 1
# view and model control variables
self.tshift = 0.05 # the amount to translate by
self.rshift = 0.05 # amount to increase rotation by
self.scaleFactor = 1000 # the amount to scale the input model by
self.default_camera_view()
self.view = np.dot(self.rotMat, self.transMat)
self.model = np.eye(4, dtype=np.float32)
self.projection = np.eye(4, dtype=np.float32)
self.GL_progs = {} # name : data for a single frame
self.apply_zoom()
gloo.set_state('translucent', clear_color='gray')
self.timer = app.Timer('auto', connect=self.on_timer, start=False)
def on_mouse_move(self, event):
if event.button == 1 and event.last_event is not None:
x0, y0 = event.last_event.pos
x1, y1 = event.pos
w, h = self.size
self.quaternion = (self.quaternion *
Quaternion(*_arcball(x0, y0, w, h)) *
Quaternion(*_arcball(x1, y1, w, h)))
self.model = self.quaternion.get_matrix()
self.program['u_model'] = self.model
self.update()
elif event.button == 2 and event.last_event is not None:
x0, y0 = event.last_event.pos
x1, y1 = event.pos
self.translate += (y1 - y0)
self.translate = max(-1, self.translate)
self.view = translate((0, 0, -self.translate), dtype=np.float32)
self.program['u_view'] = self.view
self.update()
def __init__(self):
app.Canvas.__init__(self, 'Cube', keys='interactive', size=(400, 400))
self.cube = BoxVisual(1.0, 0.5, 0.25, color='red', edge_color='black')
self.cube.transform = transforms.MatrixTransform()
self.cube.transform.scale((100, 100, 0.001))
self.cube.transform.translate((200, 200))
self.quaternion = Quaternion()
self.show()
def set_view(self, view):
"""(Re-)set camera position.
Parameters
----------
view : XY | XZ | YZ
"""
if isinstance(view, Quaternion):
q = view
elif view == 'XY':
q = Quaternion(w=1, x=.4, y=0, z=0)
elif view == 'XZ':
q = Quaternion(w=1, x=-.4, y=0, z=0)
elif view == 'YZ':
q = Quaternion(w=.6, x=0.5, y=0.5, z=-.4)
else:
raise TypeError(f'Unable to set view from {type(view)}')
self.camera3d._quaternion = q
# This is necessary to force a redraw
self.camera3d.set_range()
def transformCube(self, t, q):
quaternion = Quaternion(*q)
self._transform.reset()
self._transform.matrix = quaternion.get_matrix()
self._transform.translate(t)
x, y, z = t
self._xPos = np.append(self._xPos, x)
self._yPos = np.append(self._yPos, y)
self._zPos = np.append(self._zPos, z)
self._plot.set_data((self._xPos.transpose(), self._yPos.transpose(),
self._zPos.transpose()),
symbol=None)
def __init__(self):
app.Canvas.__init__(self, 'Cube', keys='interactive',
size=(400, 400))
self.cube = CubeVisual((1.0, 0.5, 0.25), color='red',
edge_color='black')
self.quaternion = Quaternion()
# Create a TransformSystem that will tell the visual how to draw
self.cube_transform = transforms.AffineTransform()
self.cube_transform.scale((100, 100, 0.001))
self.cube_transform.translate((200, 200))
self.tr_sys = transforms.TransformSystem(self)
self.tr_sys.visual_to_document = self.cube_transform
self.show()
def getOrientation(self):
if self.QuadState == None:
# if not connected to gs-control.py, being run as stand alone script
self.orientation_quat = (
vis_util.sample_quat(0, 0.5, 0, degrees=True) *
self.orientation_quat).normalize()
#self.orientation_quat = Quaternion(0.9659258262890683, -0.25881904510252074, 0, 0) * self.orientation_quat
return self.orientation_quat
else:
hh = self.QuadState.heading
gg = Quaternion()
gg.w = hh.w
gg.x = hh.z
gg.y = hh.y
gg.z = -hh.x
return gg.normalize()
def __init__(self, path_to_script = None):
"""Standard __init__ method.
Parameters
----------
path_to_script : str
path to script
Attributes
----------
command_series : list
list of script commands
end: int
max frame
command_list: list
list of dictionaries with each element reprenting one operation. Keys are:
'start': int
start from of operation
'end': int
start from of operation
'operation': str
type of operation, e.g. 'zoom', 'rotate'
'params': list
list of parameters needed for operation e.g [2] for 'zoom'
states_dict : list
list of dictionaries defining napari viewer states for each frame. Dictionaries have keys:
'frame': int, frame
'rotate': vispy quaternion, camera rotation
'translate': tuple, camera center
'zoom': float, camera zooming factor
'vis': list of booleans, visibility of layers
'time': int, time-point
key_frames: list
same as states_dict but only elements containing a change are conserved
"""
if path_to_script is None:
raise TypeError('You need to pass a string with a path to a script')
else:
self.path_to_script = path_to_script
#instantiate Quaternion object
self.q = Quaternion()
def Quadcopter(self, nr, nc, r, l):
quad_frame, colors = vis_util.create_quad_frame(rows=nr,
cols=nc,
radius=r,
length=l,
offset=True)
quad_mesh = vis_util.MyMeshData()
quad_mesh.set_vertices(quad_frame.get_vertices())
quad_mesh.set_faces(quad_frame.get_faces())
quad_mesh.set_vertex_colors(colors)
vertices, filled, outline = quad_mesh.get_glTriangles()
# make vertex array, etc
self.filled_buf = gloo.IndexBuffer(filled)
self.outline_buf = gloo.IndexBuffer(outline)
self.vertices_buff = gloo.VertexBuffer(vertices)
# orientation
if self.QuadState == None:
self.orientation_quat = Quaternion(1, 0, 0, 0)
def __init__(self, pdbdata, mode='cpk'):
#Startup
app.Canvas.__init__(self, keys='interactive', size=(W, H))
#Loading shaders
self.program = gloo.Program(vertex, fragment)
#Analyze pdb file
self.parser = PDBParser(QUIET=True, PERMISSIVE=True)
self.structure = self.parser.get_structure('model', pdbdata)
#Mode selection
if mode not in Canvas.visualization_modes:
raise Exception('Not recognized visualization mode %s' % mode)
self.mode = mode
#Get the data of our atoms
self.atom_information()
#Camera settings
self.translate = max(abs(np.concatenate(self.coordinates))) + 40
self.translate = max(-1, self.translate)
self.view = translate((0, 0, -self.translate), dtype=np.float32)
self.model = np.eye(4, dtype=np.float32)
self.projection = np.eye(4, dtype=np.float32)
self.program['u_projection'] = self.projection
self.quaternion = Quaternion()
#Load data depending on the mode
self.apply_zoom()
self.load_data()
#self.lines = visuals.LinePlotVisual(self.chain_coords[1], width= 5.0, marker_size= 0.0, color=self.chain_colors[1])
gloo.set_state(depth_test=True, clear_color='white')
self.show()
def __init__(self, rview, zview=None, zoom_speed=0.1, move_speed=0.01):
""" Constructor
Parameters
----------
rview : float
Radius of inscribed sphere within the field of view.
zview : float
Distance to the z-clip plane from the origin.
zoom_speed : float
How sensitive the field of view to mouse wheel rolling.
move_speed : float
How sensitive the translation of view to mouse dragging.
"""
self.rview = rview
self.zview = zview if zview else rview
self.zoom_speed = zoom_speed
self.move_speed = move_speed
self._translation = (0.0, 0.0, 0.0)
self._quaternion = Quaternion()
def __init__(self, pdbdata, mode='cpk'):
#Startup
app.Canvas.__init__(self, keys='interactive', size=(W, H))
#Loading shaders
self.program = gloo.Program(vertex, fragment)
#Analyze pdb file
self.parser = PDBParser(QUIET=True, PERMISSIVE=True)
self.structure = self.parser.get_structure('model', pdbdata)
#DSSP prediction
self.pmodel = self.structure[0]
self.dssp = DSSP(self.pmodel, pdbdata)
#Mode selection
if mode not in Canvas.visualization_modes:
raise Exception('Not recognized visualization mode %s' % mode)
self.mode = mode
#Camera settings
self.translate = 50
self.translate = max(-1, self.translate)
self.view = translate((0, 0, -self.translate), dtype=np.float32)
self.model = np.eye(4, dtype=np.float32)
self.projection = np.eye(4, dtype=np.float32)
self.program['u_projection'] = self.projection
self.quaternion = Quaternion()
#Load data depending on the mdoe
self.apply_zoom()
self.atom_information()
self.load_data()
self.show()
def estimate(quat_packet, ns_qstate):
"""
quat_packet: list of 8 raw bytes
convert to signed int16; converted to float32; update ns_qstate.heading;
"""
for i in range(0, 8, 2):
data[i // 2] = (quat_packet[i] << 8) | quat_packet[i + 1]
if data[i // 2] & 0x8000:
data[i // 2] = data[i // 2] - 0x10000
qq = np.array([
float(data[0]) / 16384.0,
float(data[1]) / 16384.0,
float(data[2]) / 16384.0,
float(data[3]) / 16384.0
],
dtype=np.float32)
# quat_packet -> data -> quaternion
# The order is weird, right?
# That's because of the way GroundStation and Quadcopter Coordinate Systems are aligned
# This weird order just converts rotation in Quad-system to GS-system
ns_qstate.heading = Quaternion(qq[0], -qq[1], -qq[3], qq[2])
ns_qstate.rpy = getRPY(ns_qstate.heading)
#print(ns_qstate.heading)
print(ns_qstate.rpy * 180.0 / np.pi)
def __init__(self, vertices, colors, K, nH, nW, faces = 0, image_grid = 0, labels = 0,
nframes = 1, edge_thresh = 1000):
app.Canvas.__init__(self, keys='interactive', size=(nW, nH))
numPnts = vertices.shape[0] / nframes
self.frame = 0
self.nframes = nframes
self.npoints = numPnts
self.has_labels = 0
self.per_frame_label = 0
self.faces_buf = {}
if( isinstance(labels, np.ndarray) ):
self.per_frame_label = 1
self.has_labels = 1
seg_colors = np.zeros([np.prod(labels.shape), 4]).astype(np.float32)
cmap = cmx.rainbow(np.linspace(0, 1, np.maximum( np.max(labels) + 1,
len(np.unique(labels) ) ) ) )
# cmap = np.array([[1.0000, 0.2857, 0, 1.0],
# [1.0000, 0.5714, 0, 1.0],
# [1.0000, 0.8571, 0, 1.0],
# [0.8571, 1.0000, 0, 1.0],
# [0.5714, 1.0000, 0, 1.0],
# [ 0, 1.0000, 0.8571, 1.0],
# [ 0, 0.8571, 1.0000, 1.0],
# [ 0, 0.5714, 1.0000, 1.0],
# [ 0, 0.2857, 1.0000, 1.0],
# [ 0, 0, 1.0000, 1.0],
# [0.2857, 0, 1.0000, 1.0],
# [0.8571, 0, 1.0000, 1.0]])
for i in np.unique(labels):
seg_colors[:,3] = 1.0
if(i != -1):
mask = labels == i
seg_colors[mask.reshape(-1),:] = cmap[i,:]
self.seg_colors = seg_colors
self.has_faces = 0
self.draw_points = 1
if( isinstance(faces, dict) ):
self.has_faces = 1
for f in range(nframes):
self.faces = faces[f]
vertices_f = vertices[f*numPnts:(f+1)*numPnts,:]
mask = self.remove_long_edges(self.faces, vertices_f, edge_thresh)
faces_temp = self.faces[mask, :]
self.faces_buf[f] = gloo.IndexBuffer(faces_temp)
elif(image_grid):
self.has_faces = 1
### we have two types of edges
indices = np.array(range(nH*nW)).astype(np.uint32).reshape((-1,1))
triangles_type1 = np.hstack((indices, indices+1, indices + nW))
triangles_type2 = np.hstack((indices, indices+nW-1, indices + nW))
mask1 = np.ones((nH,nW),dtype=bool)
mask2 = np.ones((nH,nW),dtype=bool)
mask1[:,-1] = False
mask1[-1,:] = False
mask2[:, 0] = False
mask2[-1,:] = False
self.faces = np.vstack((
triangles_type1[mask1.reshape(-1), :],
triangles_type2[mask2.reshape(-1), :]))
## all the frames use the same faces
if(self.has_labels):
if(not self.per_frame_label):
# remove connections between different labels
labels_temp = labels.reshape(-1)
mask01 = labels_temp[self.faces[:,0]] == labels_temp[self.faces[:,1]]
mask02 = labels_temp[self.faces[:,0]] == labels_temp[self.faces[:,2]]
mask12 = labels_temp[self.faces[:,1]] == labels_temp[self.faces[:,2]]
mask = np.logical_and(mask01, mask02)
mask = np.logical_and(mask, mask12)
self.faces = self.faces[mask,:]
for f in range(nframes):
vertices_f = vertices[f*numPnts:(f+1)*numPnts,:]
mask = self.remove_long_edges(self.faces, vertices_f, edge_thresh)
faces_temp = self.faces[mask, :]
self.faces_buf[f] = gloo.IndexBuffer(faces_temp)
# different frames have different faces
if(self.has_labels):
if(self.per_frame_label):
for f in range(nframes):
labels_temp = labels[f*numPnts:(f+1)*numPnts].reshape(-1)
mask01 = labels_temp[self.faces[:,0]] == labels_temp[self.faces[:,1]]
mask02 = labels_temp[self.faces[:,0]] == labels_temp[self.faces[:,2]]
mask12 = labels_temp[self.faces[:,1]] == labels_temp[self.faces[:,2]]
mask = np.logical_and(mask01, mask02)
mask = np.logical_and(mask, mask12)
faces_temp = self.faces[mask,:]
vertices_f = vertices[f*numPnts:(f+1)*numPnts,:]
mask = self.remove_long_edges(faces_temp, vertices_f, edge_thresh)
faces_temp = faces_temp[mask,:]
self.faces_buf[f] = gloo.IndexBuffer(faces_temp)
self.vertices = vertices
self.colors = np.hstack((colors,
np.ones((colors.shape[0], 1)).astype(np.float32)))
self.update_color = 0
if(colors.shape[0] == numPnts * nframes):
self.update_color = 1
# data contains the data to render
self.data = np.zeros(numPnts, [('a_position', np.float32, 3),
('a_color', np.float32, 4),
('a_seg_color', np.float32, 4)])
self.data['a_position'] = self.vertices[0:numPnts,:]
self.data['a_color'] = self.colors[0:numPnts,:]
if(self.has_labels):
self.data['a_seg_color'] = self.seg_colors[0:numPnts,:]
else:
self.data['a_seg_color'] = self.colors[0:numPnts,:]
self.center = np.mean(vertices, axis=0)
self.program = gloo.Program(vert, frag)
self.program.bind(gloo.VertexBuffer(self.data))
# self.program['a_color'] = self.data['a_color']
# self.program['a_position'] = self.data['a_position']
# self.program['a_seg_color'] = self.data['a_seg_color']
self.view = translate((0, 0, 0))
self.model = np.eye(4, dtype=np.float32)
gloo.set_viewport(0, 0, self.physical_size[0], self.physical_size[1])
# gloo.set_viewport(0, 0, nW, nH)
if(isinstance(K, np.ndarray)):
projection = projection_from_intrinsics(K, nH, nW)
else:
projection = ortho_projection(nH, nW)
#print projection
self.projection = projection.T
# self.projection = np.ascontiguousarray(projection)
# self.projection = np.ascontiguousarray(projection.T)
# self.projection = perspective(45.0, self.size[0] /
# float(self.size[1]), 2.0, 1000.0)
self.program['u_projection'] = self.projection
self.program['u_model'] = self.model
self.program['u_view'] = self.view
self.program['u_color'] = 1, 1, 1, 1
self.program['u_plot_seg'] = 0
self.program['u_point_size'] = 5
self.nH = nH
self.nW = nW
self.last_x = 0
self.last_y = 0
self.substract_center = translate((-self.center[0],
-self.center[1],
-self.center[2]))
self.add_center = translate((self.center[0],
self.center[1],
self.center[2]))
self.translate_X = self.center[0]
self.translate_Y = self.center[1]
self.translate_Z = self.center[2]
self.scale = - self.center[2] / 100
self.rot_X = 0
self.rot_Y = 0
self.rot_Z = 0
self.quaternion = Quaternion()
self.theta = 0
self.phi = 0
#gloo.set_clear_color('black')
gloo.set_clear_color('white')
gloo.set_state('opaque')
gloo.set_state(depth_test=True)
# gloo.set_polygon_offset(1, 1)
self.show()
# Serial objects are not pickle-able, hence they cannot be a part of a Manager.Namespace and thus serial port is a global.
arduino = at_talk.radio(args.port, 19200)
# There are multiple namespaces for flexibility
# {ns_comms, ns_qstate, ns_vis, ns_cfg}
mgr = multiprocessing.Manager()
ns_comms = mgr.Namespace()
ns_comms.name = "Communications:\n\tMode that rxControl operates in(comms_mode)\n\tRecieved packet from mpu(quat_packet)\n\tRecieved packet for GS(gs_packet)"
ns_comms.quat_packet = None
ns_comms.gs_packet = None
ns_comms.mode = 'att_est'
ns_qstate = mgr.Namespace()
ns_qstate.name = "QuadState:\n\tAttitude Quaternion(heading)\n\tRPY calc. from the heading(rpy)"
ns_qstate.heading = Quaternion()
ns_qstate.rpy = (0.0, 0.0, 0.0)
ns_cfg = mgr.Namespace()
ns_cfg.a_scale = 16384.0 # Accel 2g
ns_cfg.g_scale = 16.384 # Gyro 2000 dps
ns_cfg.TIME_INTERVAL = 0.02 # ms
ns_cfg.comms_active = False
ns_cfg.this_is_v2 = sys.version_info[0] == 2
# Queues
vis_canvas = visual.Canvas(800, 600, ns_qstate)
sorter = rxControl.Sorter(ns_comms, ns_cfg, ns_qstate, arduino)
ns_cfg.comms_active = True
self._yPos = np.append(self._yPos, y)
self._zPos = np.append(self._zPos, z)
self._plot.set_data((self._xPos.transpose(), self._yPos.transpose(),
self._zPos.transpose()),
symbol=None)
def resetTransform(self):
self._xPos = np.array([0], dtype=np.float32)
self._yPos = np.array([0], dtype=np.float32)
self._zPos = np.array([0], dtype=np.float32)
if __name__ == '__main__':
import sys
mp = MotionPlotter()
q1 = Quaternion()
q2 = Quaternion()
y = 0
p = 0
tx = 0
ty = 0
tz = 0
signx = -1
signy = -1
signz = -1
def update(ev):
global y, p, tx, ty, tz, q1, q2, signx, signy, signz
q1 = Quaternion.create_from_axis_angle(y, 0, 0, 1, degrees=True)
q2 = Quaternion.create_from_axis_angle(p, 0, 1, 0, degrees=True)
q = q1 * q2
