def __init__(self,
use_thread=False,
sleep_dt=0.,
verbose=False,
translation_range=(-1, 1),
rotation_range=(-1, 1)):
"""
Initialize the SpaceMouse interface.
Args:
use_thread (bool): If True, it will run the interface in a separate thread than the main one.
The interface will update its data automatically.
sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring or
setting the next sample.
verbose (bool): If True, it will print information about the state of the interface. This is let to the
programmer what he / she wishes to print.
translation_range (np.array[2], np.array[2,3], tuple of 2 float, tuple of 2 np.array[3]): the lower and
higher bounds for the (x, y, z). This is used to normalize the translation range to be between
[-1, 1]. The frame (x,y,z) is defined with x pointing forward, y to the left, and z up.
rotation_range (np.array[2], np.array[2,3], tuple of 2 float, tuple of 2 np.array[3]): the lower and higher
bounds for the roll-pitch-yaw angles. This is used to normalize the rotation range to be between
[-1, 1]. The frame (x,y,z) is defined with x pointing forward, y to the left, and z up.
"""
# Open connection to the daemon via AF_UNIX socket.
spnav.spnav_open()
# check the ranges and their shapes
translation_range = np.abs(
np.asarray(translation_range, dtype=np.float))
rotation_range = np.abs(np.asarray(rotation_range, dtype=np.float))
if translation_range.shape != (2, ) and translation_range.shape != (2,
3):
raise ValueError(
"Expecting the shape of 'translation_range' to be (2,) or (2,3), instead got: "
"{}".format(translation_range.shape))
if rotation_range.shape != (2, ) and rotation_range.shape != (2, 3):
raise ValueError(
"Expecting the shape of 'rotation_range' to be (2,) or (2,3), instead got: "
"{}".format(rotation_range.shape))
if translation_range.shape == (2, ):
translation_range = np.vstack([translation_range] * 3).T
if rotation_range.shape == (2, ):
rotation_range = np.vstack([rotation_range] * 3).T
self.translation_range = translation_range
self.rotation_range = rotation_range
# define space mouse events
self.left_button_pressed = False
self.right_button_pressed = False
self.translation = np.zeros(3)
self.rotation = np.zeros(3)
self.updated = False
super(SpaceMouseInterface, self).__init__(use_thread=use_thread,
sleep_dt=sleep_dt,
verbose=verbose)
def __init__(self, sim, eef_link='right_hand'):
self.sim = sim
self.eef_link = eef_link
# open SpaceNavigator driver
spnav.spnav_open()
self.grasp = False # False: open gripper True: close gripper
self.ctrl_mod = False # False: translation velocity True: rotational velocity
def run(self):
spnav.spnav_open()
while self.run_thread_flag:
start_time = time()
self.process_spnav_events()
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
def run(self):
spnav.spnav_open()
while self.run_thread_flag:
start_time = time()
self.process_spnav_events()
self.check_control_mode_change()
self.broadcast_control_state()
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
def run(self):
spnav.spnav_open()
while self.run_thread_flag:
start_time = time()
self.process_spnav_events()
self.check_control_mode_change()
self.send_arm_control_commands()
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff * 1000), 0))
def _start_spnav(self):
_fix_spnav_py3()
import spnav
try:
spnav.spnav_open()
except spnav.SpnavConnectionException as e:
raise SpaceNavStartError(str(e))
self._running = True
self.stop = lambda: setattr(self, '_running', False)
t = threading.Thread(target=self._spnav_event_loop)
t.daemon = True
t.start()
def run(self):
self.logger.debug("Starting SpaceNav Mouse Thread")
spnav.spnav_open()
while self.run_thread_flag:
start_time = time()
self.process_spnav_events()
self.check_control_mode_change()
self.broadcast_control_state()
time_diff = time() - start_time
# self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
self.logger.debug("Stopping SpaceNav Mouse Thread")
def main():
spnav_open()
s = open_scoket(host=HOST, port=PORT, reconnect=RECONNECT)
def process_spnav_evnt(event):
norm_event = normalize(event)
if norm_event:
json_string = json.dumps(norm_event)
send(json_string, sock=s)
def poll_loop():
event = spnav_poll_event()
if event is not None:
if event.ev_type == SPNAV_EVENT_MOTION:
process_spnav_evnt(event)
try:
while True:
poll_loop()
except KeyboardInterrupt:
print '\nQuitting...'
except socket.error, err:
print "Connection error: " + err.strerror
"""
Uses 3dconnexion w/ spacenavd
Author: Sam Torno
Rotating Cube Simulation
Modified from by Leonel Machava <*****@*****.**>'s code
"""
import sys, math, pygame, spnav
from operator import itemgetter
try:
spnav.spnav_open()
except spnav.SpnavConnectionException:
print('cannot demo 3dxconnexion, none detected')
sys.exit(1)
class Point3D:
def __init__(self, x = 0, y = 0, z = 0):
self.x, self.y, self.z = float(x), float(y), float(z)
def rotateX(self, angle):
""" Rotates the point around the X axis by the given angle in degrees. """
rad = angle * math.pi / 180
cosa = math.cos(rad)
sina = math.sin(rad)
y = self.y * cosa - self.z * sina
z = self.y * sina + self.z * cosa
return Point3D(self.x, y, z)
def rotateY(self, angle):
""" Rotates the point around the Y axis by the given angle in degrees. """
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_open_success():
m = mock_libspnav({'spnav_open': 0})
spnav.spnav_open()
assert m.spnav_open.called
def test_spnav_open_fail():
m = mock_libspnav({'spnav_open': -1})
spnav.spnav_open()
