def _spnav_event_loop(self):
import spnav
kill_pending_events = False
try:
while self._running:
if not self.focus_test():
kill_pending_events = True
self.cmd._sdof(0., 0., 0., 0., 0., 0.)
time.sleep(0.1)
continue
if kill_pending_events:
kill_pending_events = False
while spnav.spnav_poll_event() is not None:
pass
event = spnav.spnav_wait_event()
if event.ev_type == spnav.SPNAV_EVENT_MOTION:
t, r = event.translation, event.rotation
args = (t[0], -t[2], -t[1], r[0], -r[2], -r[1])
args = [v * SCALE for v in args]
self.cmd._sdof(*args)
elif event.ev_type == spnav.SPNAV_EVENT_BUTTON:
if event.press:
self.cmd._sdof_button(event.bnum + 1)
except spnav.SpnavWaitException:
print('SpnavWaitException: spacenavd stopped?')
finally:
spnav.spnav_close()
def get_spnav_cmd(self):
trans_cmd = np.zeros(3, dtype=np.float16)
rot_cmd = np.zeros(3, dtype=np.float16)
sp_event = spnav.spnav_poll_event()
if sp_event is not None and sp_event.ev_type == spnav.SPNAV_EVENT_BUTTON:
# print(sp_event.bnum, sp_event.press)
if sp_event.bnum == 0 and sp_event.press:
self.grasp = not self.grasp
print('Gripper: {}'.format('Open' if self.grasp else 'Close'))
elif sp_event.bnum == 1 and sp_event.press:
self.ctrl_mod = not self.ctrl_mod
print('Ctrl Mod: {}'.format(
'Translational' if not self.ctrl_mod else 'Rotational'))
elif sp_event is not None and sp_event.ev_type == spnav.SPNAV_EVENT_MOTION:
trans_cmd = np.asarray(sp_event.translation,
dtype=np.float16) / 1000
rot_cmd = np.asarray(sp_event.rotation, dtype=np.float16) / 500
# print('pos: {}'.format(pos_cmd[0]))
eef_vel = np.concatenate((trans_cmd, rot_cmd))
return EEFCommand(self.grasp, self.ctrl_mod, eef_vel)
def test_spnav_poll_event_button():
def button(event):
fill_button_event(event)
return 1
m = mock_libspnav({'spnav_poll_event': button})
event = spnav.spnav_poll_event()
assert event is not None
assert_equals(event.bnum, 0)
assert_equals(event.press, True)
def test_spnav_poll_event_motion():
def motion(event):
fill_motion_event(event)
return 1
m = mock_libspnav({'spnav_poll_event': motion})
event = spnav.spnav_poll_event()
assert event is not None
assert_equals(event.translation, (-1, 2, -3))
assert_equals(event.rotation, (10, -20, 30))
def run(self):
"""Run the interface."""
# # Remove pending space navigator events from the queue
# spnav.spnav_remove_events(spnav.SPNAV_EVENT_ANY)
# get space mouse event (without waiting)
# event = spnav_wait_event() # blocking wait for the next event
# try:
event = spnav.spnav_poll_event()
# except KeyboardInterrupt:
# print '\nQuitting...'
# finally:
# self.close()
# check the event
self.updated = True
if isinstance(event, spnav.SpnavButtonEvent): # button event
if event.bnum == 0: # left
self.left_button_pressed = event.press
elif event.bnum == 1: # right
self.right_button_pressed = event.press
elif isinstance(event, spnav.SpnavMotionEvent): # mouse motion event
# translation
x, y, z = event.translation # x, y, z in spnav
self.translation = np.array(
[z, -x, y], dtype=np.float) # x (front), y (left), z (up)
condition = self.translation < 0
self.translation[condition] /= self.translation_range[0][condition]
self.translation[~condition] /= self.translation_range[1][
~condition]
# rotation
r, p, y = event.rotation # r, p, y in spnav
self.rotation = np.array(
[y, -r, p], dtype=np.float) # x (front), y (left), z (up)
condition = self.rotation < 0
self.rotation[condition] /= self.rotation_range[0][condition]
self.rotation[~condition] /= self.rotation_range[1][~condition]
else:
self.updated = False
# if we should print the events
if self.verbose and self.updated:
print("Left and Right buttons: {} and {}".format(
self.left_button_pressed, self.right_button_pressed))
print("Space mouse translation and orientation: {} and {}".format(
self.translation, self.rotation))
def process_spnav_events(self):
event = spnav.spnav_poll_event()
if event:
if event.ev_type == spnav.SPNAV_EVENT_MOTION:
self.spnav_states["linear_x"] = event.translation[0] / 350.0
self.spnav_states["linear_y"] = event.translation[2] / 350.0
self.spnav_states["linear_z"] = event.translation[1] / 350.0
self.spnav_states["angular_x"] = event.rotation[2] / 350.0
self.spnav_states["angular_y"] = -(event.rotation[0] / 350.0)
self.spnav_states["angular_z"] = -(event.rotation[1] / 350.0)
# print "x", self.spnav_states["linear_x"], "\t", "y", self.spnav_states["linear_y"], "\t", "z", self.spnav_states["linear_z"]
# print "x", self.spnav_states["angular_x"], "\t", "y", self.spnav_states["angular_y"], "\t", "z", self.spnav_states["angular_z"]
else:
self.spnav_states[self.event_mapping_to_button_mapping[
event.bnum]] = event.press
def process_spnav_events(self):
event = spnav.spnav_poll_event()
if event:
if event.ev_type == spnav.SPNAV_EVENT_MOTION:
# FIXME: Make sure these divisors are correct. Should be for most. Check linear_z, is weird
self.spnav_states["linear_x"] = event.translation[0] / 350.0
self.spnav_states["linear_y"] = event.translation[2] / 350.0
self.spnav_states["linear_z"] = event.translation[1] / 350.0
self.spnav_states["angular_x"] = event.rotation[2] / 350.0
self.spnav_states["angular_y"] = event.rotation[0] / 350.0
self.spnav_states["angular_z"] = event.rotation[1] / 350.0
# print "x", self.spnav_states["linear_x"], "\t", "y", self.spnav_states["linear_y"], "\t", "z", self.spnav_states["linear_z"]
# print "x", self.spnav_states["angular_x"], "\t", "y", self.spnav_states["angular_y"], "\t", "z", self.spnav_states["angular_z"]
else:
self.spnav_states[self.event_mapping_to_button_mapping[
event.bnum]] = event.press
def loop(self):
ts = 0
btn0_state = 0
btn_event = None
cmd = Cmd()
while True:
time.sleep(0.01)
cmd.reset()
#spnav.spnav_remove_events(spnav.SPNAV_EVENT_ANY) # seems broken
event = spnav.spnav_poll_event()
if event:
if type(event) is spnav.SpnavButtonEvent:
btn_event = event
if event.bnum == 0:
btn0_state = event.press
elif type(event) is spnav.SpnavMotionEvent:
if abs(event.translation[0]) > 30:
cmd.y = event.translation[0] / self.lin_coef
if abs(event.translation[1]) > 30:
cmd.z = -1 * event.translation[1] / self.lin_coef
if abs(event.translation[2]) > 30:
cmd.x = event.translation[2] / self.lin_coef
if abs(event.rotation[0]) > 20:
cmd.ry = event.rotation[0] / self.lin_coef
if abs(event.rotation[1]) > 20:
cmd.rz = -1 * event.rotation[1] / self.lin_coef
if abs(event.rotation[2]) > 20:
cmd.rx = event.rotation[2] / self.lin_coef
if (time.time() - ts) > 0.12:
ts = time.time()
speeds = cmd.get_speeds()
if btn0_state:
self.robot.speedl_tool(speeds, acc=0.10, min_time=2)
else:
self.robot.speedl(speeds, acc=0.10, min_time=2)
btn_event = None
speeds = cmd.get_speeds()
#if speeds != [0 for _ in speeds]:
print(event)
print("Sending", speeds)
def loop(self):
ts = 0
btn0_state = 0
btn_event = None
cmd = Cmd()
while True:
time.sleep(0.01)
cmd.reset()
#spnav.spnav_remove_events(spnav.SPNAV_EVENT_ANY) # seems broken
event = spnav.spnav_poll_event()
if event:
if type(event) is spnav.SpnavButtonEvent:
btn_event = event
if event.bnum == 0:
btn0_state = event.press
elif type(event) is spnav.SpnavMotionEvent:
if abs(event.translation[0]) > 30:
cmd.y = event.translation[0] / self.lin_coef
if abs(event.translation[1]) > 30:
cmd.z = -1 * event.translation[1] / self.lin_coef
if abs(event.translation[2]) > 30:
cmd.x = event.translation[2] / self.lin_coef
if abs(event.rotation[0]) > 20:
cmd.ry = event.rotation[0] / self.lin_coef
if abs(event.rotation[1]) > 20:
cmd.rz = -1 * event.rotation[1] / self.lin_coef
if abs(event.rotation[2]) > 20:
cmd.rx = event.rotation[2] / self.lin_coef
if (time.time() - ts) > 0.12:
ts = time.time()
speeds = cmd.get_speeds()
if btn0_state:
self.robot.speedl_tool(speeds, acc=0.10, min_time=2)
else:
self.robot.speedl(speeds, acc=0.10, min_time=2)
btn_event = None
speeds = cmd.get_speeds()
#if speeds != [0 for _ in speeds]:
print(event)
print("Sending", speeds)
def run(self):
""" Main Loop """
ret = None
while ret == None:
for event in pygame.event.get():
if event.type == pygame.QUIT:
ret = pygame.QUIT
continue
spnav_event = spnav.spnav_poll_event()
if spnav_event is not None:
if isinstance(spnav_event, spnav.SpnavMotionEvent):
evt = {
'period': spnav_event.period,
'rotation': spnav_event.rotation,
'translation': spnav_event.translation
}
self.old_evt = self.evt
self.evt = evt
self.old_delta = self.delta
self.delta['period'] = evt['period']
try:
self.delta['period'] = self.delta['period'] / self.old_delta['period']
except:
pass
self.delta['x'] += evt['rotation'][0] * self.old_delta['period']
self.delta['y'] += evt['rotation'][1] * self.old_delta['period']
self.delta['z'] += evt['rotation'][2] * self.old_delta['period']
spnav.spnav_remove_events(spnav.SPNAV_EVENT_MOTION)
elif isinstance(spnav_event, spnav.SpnavButtonEvent):
ret = spnav.SpnavButtonEvent
continue
self.clock.tick(50)
self.screen.fill((0,32,0))
# It will hold transformed vertices.
t = []
for v in self.vertices:
# Rotate the point around X axis, then around Y axis, and finally around Z axis.
r = v.rotateX(self.delta['x']).rotateY(self.delta['y']).rotateZ(self.delta['z'])
# Transform the point from 3D to 2D
p = r.project(self.screen.get_width(), self.screen.get_height(), 256, 4)
# Put the point in the list of transformed vertices
t.append(p)
# Calculate the average Z values of each face.
avg_z = []
i = 0
for f in self.faces:
z = (t[f[0]].z + t[f[1]].z + t[f[2]].z + t[f[3]].z) / 4.0
avg_z.append([i,z])
i = i + 1
# Draw the faces using the Painter's algorithm:
# Distant faces are drawn before the closer ones.
for tmp in sorted(avg_z,key=itemgetter(1),reverse=True):
face_index = tmp[0]
f = self.faces[face_index]
pointlist = [(t[f[0]].x, t[f[0]].y), (t[f[1]].x, t[f[1]].y),
(t[f[1]].x, t[f[1]].y), (t[f[2]].x, t[f[2]].y),
(t[f[2]].x, t[f[2]].y), (t[f[3]].x, t[f[3]].y),
(t[f[3]].x, t[f[3]].y), (t[f[0]].x, t[f[0]].y)]
pygame.draw.polygon(self.screen,self.colors[face_index], pointlist)
pygame.display.flip()
# gracefully exit
spnav.spnav_close()
if ret == pygame.QUIT:
print('Closed the window?')
pass
if ret == spnav.SpnavButtonEvent:
print('Clicked button to exit.')
pass
pygame.quit()
sys.exit()
from spnav import spnav_open, spnav_poll_event, spnav_close
if __name__ == '__main__':
spnav_open()
try:
while True:
event = spnav_poll_event()
if event is not None:
print event
except KeyboardInterrupt:
print '\nQuitting...'
finally:
spnav_close()
def test_spnav_poll_event_none():
m = mock_libspnav({'spnav_poll_event': 0})
assert spnav.spnav_poll_event() is None
def poll_loop():
event = spnav_poll_event()
if event is not None:
if event.ev_type == SPNAV_EVENT_MOTION:
process_spnav_evnt(event)
