95 lines
2.7 KiB
Python
95 lines
2.7 KiB
Python
from __future__ import division
|
|
|
|
import spnav
|
|
import time
|
|
import math3d as m3d
|
|
from math import pi
|
|
|
|
import urx
|
|
|
|
class Cmd(object):
|
|
def __init__(self):
|
|
self.reset()
|
|
|
|
def reset(self):
|
|
self.x = 0
|
|
self.y = 0
|
|
self.z = 0
|
|
self.rx = 0
|
|
self.ry = 0
|
|
self.rz = 0
|
|
self.btn0 = 0
|
|
self.btn1 = 0
|
|
|
|
def get_speeds(self):
|
|
return [self.x, self.y, self.z, self.rx, self.ry, self.rz]
|
|
|
|
|
|
|
|
class Service(object):
|
|
def __init__(self, robot):
|
|
self.robot = robot
|
|
self.lin_coef = 5000
|
|
self.rot_coef = 5000
|
|
|
|
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)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
spnav.spnav_open()
|
|
robot = urx.Robot("192.168.0.90")
|
|
#robot = urx.Robot("localhost")
|
|
robot.set_tcp((0, 0, 0.27, 0, 0, 0))
|
|
trx = m3d.Transform()
|
|
trx.orient.rotate_zb(pi/4)
|
|
robot.set_csys("mycsys", trx)
|
|
service = Service(robot)
|
|
try:
|
|
service.loop()
|
|
finally:
|
|
robot.close()
|
|
spnav.spnav_close()
|
|
|
|
|
|
|