def trackrotation(motive_filename, body):
motive.initialize()
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
body = motive.get_rigid_bodies()[body]
for el in range(3):
body.reset_orientation()
motive.update()
window = RotationWindow()
window.mesh.position.z = -1.5
def update_body(dt, window, body):
motive.update()
window.mesh.rotation.xyzw = body.rotation_quats
fmt_str = "loc: {:.1f}, {:.1f}, {:.1f}\t rot: {:.2f}, {:.2f}, {:.2f}, {:.2f}"
window.label.text = fmt_str.format(*(body.location +
body.rotation_quats))
pyglet.clock.schedule(update_body, window, body)
pyglet.app.run()
def latency_test(motive_filename, projector_filename, port, body, screen):
with serial.Serial(port=port, timeout=.5) as device:
device.write('A')
motive.initialize()
motive.load_project(motive_filename.encode())
motive.update()
body = motive.get_rigid_bodies()[body]
# Load projector's Camera object, created from the calib_projector ratcave_utils CLI tool.
projector = rc.Camera.from_pickle(projector_filename.encode())
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = LatencyDisplayApp(projector=projector, serial_device=device, fullscreen=True, screen=screen)
def update_body(dt, window, body):
motive.update()
window.dot.position.xyz = body.location
pyglet.clock.schedule(update_body, window, body)
pyglet.app.run()
def trackbody(motive_filename, body):
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
body = motive.get_rigid_bodies()[body]
while True:
motive.update()
click.echo(body.location)
parser.add_argument('-i', action='store', dest='motive_projectfile', default=motive.utils.backup_project_filename,
help='Name of the motive project file to load. If not used, will load most recent Project file loaded in MotivePy.')
args = parser.parse_args()
# Select Rigid Body to track.
motive.load_project(args.motive_projectfile)
print("Loaded Motive Project: {}".format(args.motive_projectfile))
hardware.motive_camera_vislight_configure()
print("Camera Settings changed to Detect Visible light:")
print("\t"+"\n\t".join(['{}: FPS={}, Gain={}, Exp.={}, Thresh.={}'.format(cam.name, cam.frame_rate, cam.image_gain, cam.exposure, cam.threshold) for cam in motive.get_cams()]))
motive.update()
rigid_bodies = motive.get_rigid_bodies()
try:
if not args.rigid_body_name:
assert len(rigid_bodies) == 1, "Only one rigid body should be present for auto-selection. Please use the -r flag to specify a rigid body name to track for the arena."
arena_name = args.rigid_body_name if args.rigid_body_name in rigid_bodies else rigid_bodies.keys()[0]
except IndexError:
raise IndexError("No Rigid Bodies found in Optitrack tracker.")
except KeyError:
raise KeyError("Rigid Body '{}' not found in list of Optitrack Rigid Bodies.".format(arena_name))
print('Arena Name: {}. N Markers: {}'.format(arena_name, len(rigid_bodies[arena_name].markers)))
assert len(rigid_bodies[arena_name].markers) > 5, "At least 6 markers in the arena's rigid body is required"
# TODO: Fix bug that requires scanning be done in original orientation (doesn't affect later recreation, luckily.)
for attempt in range(3): # Sometimes it doesn't work on the first try, for some reason.
rigid_bodies[arena_name].reset_orientation()
def view_arenafit(motive_filename, projector_filename, arena_filename, screen):
# """Displays mesh in .obj file. Useful for checking that files are rendering properly."""
reader = rc.WavefrontReader(arena_filename)
arena = reader.get_mesh('Arena', mean_center=True)
arena.rotation = arena.rotation.to_quaternion()
print('Arena Loaded. Position: {}, Rotation: {}'.format(arena.position, arena.rotation))
camera = rc.Camera.from_pickle(projector_filename)
camera.projection.fov_y = 39
light = rc.Light(position=(camera.position.xyz))
root = rc.EmptyEntity()
root.add_child(arena)
sphere = rc.WavefrontReader(rc.resources.obj_primitives).get_mesh('Sphere', scale=.05)
root.add_child(sphere)
scene = rc.Scene(meshes=root,
camera=camera,
light=light,
bgColor=(.2, .4, .2))
scene.gl_states = scene.gl_states[:-1]
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = pyglet.window.Window(fullscreen=True, screen=screen)
label = pyglet.text.Label()
fps_display = FPSDisplay(window)
shader = rc.Shader.from_file(*rc.resources.genShader)
@window.event
def on_draw():
with shader:
scene.draw()
# label.draw()
# window.clear()
fps_display.draw()
@window.event
def on_resize(width, height):
camera.projection.aspect = float(width) / height
@window.event
def on_key_release(sym, mod):
if sym == key.UP:
scene.camera.projection.fov_y += .5
elif sym == key.DOWN:
scene.camera.projection.fov_y -= .5
import motive
motive.initialize()
motive.load_project(motive_filename.encode())
motive.update()
rb = motive.get_rigid_bodies()['Arena']
# for el in range(3):
# rb.reset_orientation()
def update_arena_position(dt):
motive.update()
arena.position.xyz = rb.location
arena.rotation.xyzw = rb.rotation_quats
arena.update()
sphere.position.xyz = rb.location
label.text = "aspect={}, fov_y={}, ({:2f}, {:2f}, {:2f}), ({:2f}, {:2f}, {:2f})".format(scene.camera.projection.aspect,
scene.camera.projection.fov_y, *(arena.position.xyz + rb.location))
pyglet.clock.schedule(update_arena_position)
pyglet.app.run()
from ratcave.graphics import *
from ratcave.utils import rotate_to_var
import numpy as np
import argparse
from ball_simulation import Bouncer
np.set_printoptions(precision=2, suppress=True)
import motive
motive.load_project("vr_demo.ttp")
motive.update()
# Create Arena and cube
reader = WavefrontReader(ratcave.graphics.resources.obj_arena)
arena = reader.get_mesh('Arena', lighting=True, centered=False)
arena_rb = motive.get_rigid_bodies()['Arena']
player = motive.get_rigid_bodies()['CalibWand']
wand = motive.get_rigid_bodies()['CalibWand']
# Calculate Arena's orientation
for attempt in range(3):
arena_rb.reset_orientation()
motive.update()
arena_markers = np.array(arena_rb.point_cloud_markers)
additional_rotation = rotate_to_var(arena_markers)
# Create Virtual Objects
reader = WavefrontReader('vr_demo.obj')
meshes = {name:reader.get_mesh(name, lighting=True, centered=False) for name in reader.mesh_names}
meshes['Arena'].visible = False
meshes['StarGrid'].drawstyle = 'point'
def display(optitrack_ip="127.0.0.1", calib_object_name=''):
# Connect to Optitrack
# tracker = ratcave.devices.Optitrack(client_ip=optitrack_ip)
# Create Arena and cube
reader = WavefrontReader(ratcave.graphics.resources.obj_arena)
arena = reader.get_mesh('Arena', lighting=True, centered=False)
arena.load_texture(ratcave.graphics.resources.img_colorgrid)
meshes = [arena]
if calib_object_name:
reader = WavefrontReader(ratcave.graphics.resources.obj_primitives)
cube = reader.get_mesh('Sphere', lighting=True, scale=.02, centered=True)
meshes.append(cube)
# Create Scene and Window
scene = Scene(meshes)
scene.camera = projector
scene.camera.fov_y = 27.8
scene.light.position = scene.camera.position
window = Window(scene, screen=1, fullscr=True)
# Update Everything's Position
# arena.world.position = tracker.rigid_bodies['Arena'].position
# arena.world.rotation = tracker.rigid_bodies['Arena'].rotation_pca_y
#
print(motive.get_rigid_bodies())
arena_rb = motive.get_rigid_bodies()['Arena']
arena.world.position = arena_rb.location
arena.world.rotation = arena_rb.rotation
motive.update()
for attempt in range(3):
arena_rb.reset_orientation()
motive.update()
markers = np.array(arena_rb.point_cloud_markers)
additional_rotation = rotate_to_var(markers)
# Print the Following every time a key is detected:
print "Camera settings:\n -shift: {0}, {1}\n -position: {2}\n -fov_y(xz): {3}\n -rotation: {4}\n\n".format(
scene.camera.x_shift, scene.camera.y_shift, scene.camera.position, scene.camera.fov_y, scene.camera.rotation)
print "Arena settings:\n -local\n\tPosition: {}\n\tRotation: {}\n -world\n\tPosition: {}\n\tRotation: {}".format(
arena.local.position, arena.local.rotation, arena.world.position, arena.world.rotation)
aa = 0
while True:
# Update Everything's Position
motive.update()
arena.world.position = arena_rb.location
arena.world.rotation = arena_rb.rotation_global
arena.world.rotation[1] += additional_rotation
arena.world.rotation[1] += aa
# If there's another object to track, then track it.
if calib_object_name:
cube.local.position = motive.get_rigid_bodies()[calib_object_name].location
# Re-Draw Everything
window.draw()
window.flip()
keys = event.getKeys()
if 'escape' in keys:
window.close()
break
elif 'up' in keys:
scene.camera.fov_y += .1
print('fov_y: {}'.format(scene.camera.fov_y))
elif 'down' in keys:
scene.camera.fov_y -= .1
print('fov_y: {}'.format(scene.camera.fov_y))
elif 'space' in keys:
aa += 180
def scan_arena(motive_filename, output_filename, body, nomeancenter, nopca,
nsides):
"""Runs Arena Scanning algorithm."""
output_filename = output_filename + '.obj' if not path.splitext(
output_filename)[1] else output_filename
assert path.splitext(output_filename)[
1] == '.obj', "Output arena filename must be a Wavefront (.obj) file"
# Load Motive Project File
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
hardware.motive_camera_vislight_configure()
motive.update()
# Get Arena's Rigid Body
rigid_bodies = motive.get_rigid_bodies()
assert body in rigid_bodies, "RigidBody {} not found in project file. Available body names: {}".format(
body, list(rigid_bodies.keys()))
assert len(
rigid_bodies[body].markers
) > 5, "At least 6 markers in the arena's rigid body is required. Only {} found".format(
len(rigid_bodies[body].markers))
# TODO: Fix bug that requires scanning be done in original orientation (doesn't affect later recreation, luckily.)
for attempt in range(
3): # Sometimes it doesn't work on the first try, for some reason.
rigid_bodies[body].reset_orientation()
motive.update()
if sum(np.abs(rigid_bodies[body].rotation)) < 1.:
break
else:
raise ValueError(
"Rigid Body Orientation not Resetting to 0,0,0 after 3 attempts. This happens sometimes (bug), please just run the script again."
)
# Scan points
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[1]
window = GridScanWindow(screen=screen, fullscreen=True)
pyglet.app.run()
points = np.array(window.marker_pos)
assert (
len(points) > 100
), "Only {} points detected. Tracker is not detecting enough points to model. Is the projector turned on?".format(
len(points))
assert points.ndim == 2
# Rotate all points to be mean-centered and aligned to Optitrack Markers direction or largest variance.
markers = np.array(rigid_bodies[body].point_cloud_markers)
if not nomeancenter:
points -= np.mean(markers, axis=0)
if not nopca:
points = np.dot(
points,
rotation_matrix(np.radians(orienting.rotate_to_var(markers)),
[0, 1, 0])[:3, :3])
# Plot preview of data collected
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*points.T)
plt.show()
# Get vertex positions and normal directions from the collected data.
vertices, normals = pointcloud.meshify(points, n_surfaces=nsides)
vertices = {
wall: pointcloud.fan_triangulate(pointcloud.reorder_vertices(verts))
for wall, verts in vertices.items()
} # Triangulate
# Write wavefront .obj file to app data directory and user-specified directory for importing into Blender.
wave_str = pointcloud.to_wavefront(body, vertices, normals)
with open(output_filename, 'wb') as wavfile:
wavfile.write(wave_str)
# Show resulting plot with points and model in same place.
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*points[::12, :].T)
for idx, verts in vertices.items():
ax.plot(*np.vstack((verts, verts[0, :])).T)
plt.show()
def scan_arena(motive_filename, output_filename, body, nomeancenter, nsides, screen):
"""Runs Arena Scanning algorithm."""
output_filename = output_filename + '.obj' if not path.splitext(output_filename)[1] else output_filename
assert path.splitext(output_filename)[1] == '.obj', "Output arena filename must be a Wavefront (.obj) file"
# Load Motive Project File
motive_filename = motive_filename.encode()
motive.initialize()
motive.load_project(motive_filename)
# Get old camera settings before changing them, to go back to them before saving later.
cam_settings = [cam.settings for cam in motive.get_cams()]
frame_rate_old = motive.get_cams()[0].frame_rate
hardware.motive_camera_vislight_configure()
motive.update()
# Get Arena's Rigid Body
rigid_bodies = motive.get_rigid_bodies()
assert body in rigid_bodies, "RigidBody {} not found in project file. Available body names: {}".format(body, list(rigid_bodies.keys()))
# assert len(rigid_bodies[body].markers) > 5, "At least 6 markers in the arena's rigid body is required. Only {} found".format(len(rigid_bodies[body].markers))
for el in range(3):
rigid_bodies[body].reset_orientation()
rigid_bodies[body].reset_pivot_offset()
motive.update()
assert np.isclose(np.array(rigid_bodies[body].rotation), 0).all(), "Orientation didn't reset."
assert np.isclose(np.array(rigid_bodies[body].location), np.mean(rigid_bodies[body].point_cloud_markers, axis=0)).all(), "Pivot didn't reset."
print("Location and Orientation Successfully reset.")
# Scan points
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = GridScanWindow(screen=screen, fullscreen=True)
pyglet.app.run()
points = np.array(window.marker_pos)
assert(len(points) > 100), "Only {} points detected. Tracker is not detecting enough points to model. Is the projector turned on?".format(len(points))
assert points.ndim == 2
# Plot preview of data collected
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*points.T)
plt.show()
# Get vertex positions and normal directions from the collected data.
vertices, normals = pointcloud.meshify_arena(points, n_surfaces=nsides)
vertices, face_indices = pointcloud.face_index(vertices)
face_indices = pointcloud.fan_triangulate(face_indices)
# Reapply old camera settings, then save.
for setting, cam in zip(cam_settings, motive.get_cams()):
cam.settings = setting
cam.image_gain = 1
cam.frame_rate = frame_rate_old
if 'Prime 13' in cam.name:
cam.set_filter_switch(True)
motive.update()
# me
if not nomeancenter:
# import ipdb
# ipdb.set_trace()
vertmean = np.mean(vertices[face_indices.flatten(), :], axis=0)
# vertmean = np.array([np.mean(np.unique(verts)) for verts in vertices.T]) # to avoid counting the same vertices twice.
vertices -= vertmean
points -= vertmean
print('Old Location: {}'.format(rigid_bodies[body].location))
arena = rigid_bodies[body]
for attempt in range(300):
print('Trying to Set New Rigid Body location, attempt {}...'.format(attempt))
arena.reset_pivot_offset()
arena.location = vertmean
if np.isclose(arena.location, vertmean, rtol=.001).all():
break
else:
raise ValueError('Motive failed to properly shift pivot to center of mesh')
print('Vertex Mean: {}'.format(vertmean))
print('New Location: {}'.format(arena.location))
# Write wavefront .obj file to app data directory and user-specified directory for importing into Blender.
writer = WavefrontWriter.from_indexed_arrays(body, vertices, normals, face_indices)
with open(output_filename, 'w') as f:
writer.dump(output_filename)
# Show resulting plot with points and model in same place.
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*points[::12, :].T)
ax.scatter(*vertices.T, c='r')
plt.show()
# motive.save_project(motive_filename)
motive.save_project(path.splitext(motive_filename)[0]+'_scanned.ttp')
def trackposition(motive_filename, projector_filename, body, screen):
motive.initialize()
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
body = motive.get_rigid_bodies()[body]
for el in range(3):
body.reset_orientation()
motive.update()
# Load projector's Camera object, created from the calib_projector ratcave_utils CLI tool.
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = RotationWindow(fullscreen=True, screen=screen)
keys = key.KeyStateHandler()
window.push_handlers(keys)
camera = rc.Camera.from_pickle(projector_filename.encode())
# camera.projection.fov_y = 38.5
window.scene.camera = camera
window.scene.light.position.xyz = camera.position.xyz
shader = rc.Shader.from_file(*rc.resources.genShader)
@window.event
def on_draw():
with shader:
window.scene.draw()
window.label.draw()
def update_body(dt, window, body):
motive.update()
window.mesh.rotation.xyzw = body.rotation_quats
window.mesh.position.xyz = body.location
# window.scene.camera.rotation.x += 15 * dt
fmt_str = "loc: {:.1f}, {:.1f}, {:.1f}\t rot: {:.2f}, {:.2f}, {:.2f}, {:.2f}\nfov_y: {fov_y:.2f}\taspect: {aspect:.2f}\nfps: {fps:.1f}"
window.label.text = fmt_str.format(
*(body.location + body.rotation_quats),
fov_y=window.scene.camera.projection.fov_y,
aspect=window.scene.camera.projection.aspect,
fps=1. / (dt + .00000001))
def update_fov(dt):
camera.projection.aspect = window.width / float(window.height)
camera.projection.update()
speed = 10. # How fast to change values on keyboard hold.
if keys[key.UP]:
camera.projection.fov_y += speed * dt
camera.projection.update()
if keys[key.DOWN]:
camera.projection.fov_y -= speed * dt
camera.projection.update()
if keys[key.LEFT]:
camera.projection.aspect += speed * dt
camera.projection.update()
if keys[key.RIGHT]:
camera.projection.aspect -= speed * dt
camera.projection.update()
pyglet.clock.schedule(update_fov)
pyglet.clock.schedule(update_body, window, body)
pyglet.app.run()
def vr_demo(motive_filename, projector_filename, arena_filename, body, screen):
motive.initialize()
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
body = motive.get_rigid_bodies()[body]
for el in range(3):
body.reset_orientation()
motive.update()
arena_body = motive.get_rigid_bodies()['Arena']
# Load projector's Camera object, created from the calib_projector ratcave_utils CLI tool.
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = pyglet.window.Window(fullscreen=False, screen=screen, vsync=False)
fbo = rc.FBO(rc.TextureCube(width=4096, height=4096))
shader3d = rc.Shader.from_file(*rc.resources.genShader)
reader = rc.WavefrontReader(rc.resources.obj_primitives)
mesh = reader.get_mesh('Monkey', scale=.022, position=(0, 0, .3))
mesh.position.y = .6
mesh.uniforms['ambient'] = .15, .15, .15
# mesh.rotation = mesh.rotation.to_quaternion()
arena = rc.WavefrontReader(arena_filename.encode()).get_mesh('Arena')
arena.rotation = arena.rotation.to_quaternion()
arena.texture = fbo.texture
floor = reader.get_mesh('Plane',
scale=50,
position=(0, 0, 0),
mean_center=True)
# floor.scale.x = 20
floor.rotation.x = -90
floor.texture = fbo.texture
# floor.rotation.x = 180
vr_scene = rc.Scene(meshes=[mesh], bgColor=(0., .3, 0))
vr_scene.camera.projection.fov_y = 90
vr_scene.camera.projection.aspect = 1.
vr_scene.camera.projection.z_near = .005
vr_scene.camera.projection.z_far = 2.
fbo_aa = rc.FBO(rc.Texture(width=4096, height=4096, mipmap=True))
quad = rc.gen_fullscreen_quad()
quad.texture = fbo_aa.texture
shader_deferred = rc.Shader.from_file(*rc.resources.deferredShader)
scene = rc.Scene(meshes=[arena, floor], bgColor=(0., 0., .3))
camera = rc.Camera.from_pickle(projector_filename.encode())
camera.projection.fov_y = 39
scene.camera = camera
scene.light.position.xyz = camera.position.xyz
vr_scene.light.position.xyz = camera.position.xyz
@window.event
def on_draw():
with shader3d:
with fbo:
vr_scene.camera.projection.match_aspect_to_viewport()
vr_scene.draw360_to_texture(fbo.texture)
scene.camera.projection.match_aspect_to_viewport()
with fbo_aa:
scene.draw()
with shader_deferred:
quad.draw()
def update_body(dt, body):
motive.update()
mesh.position.xyz = arena_body.location
mesh.position.y -= .07
# mesh.rotation.xyzw = arena_body.rotation_quats
# mesh.rotation.y += 10 * dt
arena.position.xyz = arena_body.location
arena.rotation.xyzw = arena_body.rotation_quats
vr_scene.camera.position.xyz = body.location
scene.camera.uniforms['playerPos'] = body.location
pyglet.clock.schedule(update_body, body)
pyglet.app.run()
def show_viewer():
"""Creates a widget showing a live video plotting the 3D position of unidentified markers (light blue) and
rigid body markers, with a grey grid denoting the floor
"""
# Create Qt Window
app = QtGui.QApplication([]) # create the graphing application
w = gl.GLViewWidget() # create widget
w.opts['distance'] = 4 # start distance from where one looks at the plot
w.setFixedSize(1100, 800)
# Initialize/Set unidentified_markers plot with dummy data.
unident_markers = gl.GLScatterPlotItem(pos=np.array([[0, 0, 0]]), color=(204/255, 1, 1, 0.8), size=6)
w.addItem(unident_markers)
# Initialize the Rigid Body Scatterplot and assign plot color to the rigid bodies
m.update()
rigs = m.get_rigid_bodies()
color_dict = {'Red': (1., 0., 0.), 'Green': (0., 1., 0.), 'Yellow': (1., 1., 0.),
'Mag.': (1., 0., 1.), 'Orange': (1., .4, 0.)}
for rig, color_name in zip(rigs.values(), itertools.cycle(color_dict)):
rig.color_name = color_name
w.addItem(gl.GLScatterPlotItem(pos=np.array([[0, 0, 0]]), color=color_dict[color_name] + (1.,), size=8))
# Make floor rectangle
grid_points = np.linspace(-1, 1, 200)
points_2d = np.array(list(itertools.product(grid_points, grid_points)))
points_3d = np.insert(points_2d, 1, 0, axis=1)
w.addItem(gl.GLScatterPlotItem(pos=points_3d, color=(0.5, 0.5, 0.5, 0.3), size=0.1))
# Rotate Everything so Y axis is up when plotted.
[item.rotate(90, 1, 0, 0) for item in w.items]
# Show widget (for different backgroundcolor see PyQtshowmarkers.py)
w.show()
# Main Draw Loop (as generator)
def update_generator():
last_time= time.time()
rig_data = ', '.join(['{0}: {1}'.format(body.name, body.color_name) for body in rigs.values()])
while True:
m.update()
# Update Title with new FPS
try:
fps = round(1. / (time.time() - last_time))
last_time = time.time()
w.setWindowTitle('MotivePy Viewer. Rigid Bodies = {{{rigid_bodies}}}. Update Rate: {fps} fps'.format(rigid_bodies = rig_data, fps=fps))
except ZeroDivisionError:
pass
# Plot
markers = m.get_unident_markers()
if markers:
unident_markers.setData(pos=np.array(m.get_unident_markers()))
for rig, scat in zip(rigs.values(), w.items[1:-1]):
scat.setData(pos=np.array(rig.point_cloud_markers))
# Return Nothing
yield
t = QtCore.QTimer()
t.timeout.connect(update_generator().next)
t.start(2)
# Start Viewer App
QtGui.QApplication.instance().exec_()
