added 2 examples files for joystick and 3d mouse control

examples/joystick_control.py

@@ -0,0 +1,139 @@
+"""
+example program to control a universal robot with a joystick
+"""
+
+import time
+
+import pygame
+
+import math3d as m3d
+from math import pi
+
+import urx
+
+class Cmd(object):
+    def __init__(self):
+        self.reset()
+
+    def reset(self):
+        self.axis0 = 0
+        self.axis1 = 0
+        self.axis2 = 0
+        self.axis3 = 0
+        self.axis4 = 0
+        self.axis5 = 0
+        self.btn0 = 0
+        self.btn1 = 0
+        self.btn2 = 0
+        self.btn3 = 0
+        self.btn4 = 0
+        self.btn5 = 0
+        self.btn6 = 0
+        self.btn7 = 0
+        self.btn8 = 0
+        self.btn9 = 0
+
+
+class Service(object):
+    def __init__(self, robot, linear_velocity=0.1, rotational_velocity=0.1, acceleration=0.1):
+        self.joystick = None
+        self.robot = robot
+        #max velocity and acceleration to be send to robot
+        self.linear_velocity = linear_velocity
+        self.rotational_velocity = rotational_velocity
+        self.acceleration = acceleration
+        #one button send the robot to a preprogram position defined by this variable in join space
+        self.init_pose = [-2.0782002408411593, -1.6628931459654561, 2.067930303382134, -1.9172217394630149, 1.5489023943220621, 0.6783171005488982]
+        
+        self.cmd = Cmd()
+
+    def init_joystick(self):
+        pygame.init()
+        self.joystick = pygame.joystick.Joystick(0)
+        self.joystick.init()
+        print('Initialized Joystick : %s' % self.joystick.get_name())
+
+    def loop(self):
+        print("Starting loop")
+        air = False
+        while True:
+            self.cmd.reset()
+            pygame.event.pump()#Seems we need polling in pygame...
+            
+            #get joystick state
+            for i in range(0, self.joystick.get_numaxes()):
+                if abs(self.joystick.get_axis(i)) > 0.2:
+                    tmp = "self.cmd.axis" + str(i) + " = " + str(self.joystick.get_axis(i))
+                    print(tmp)
+                    exec(tmp)
+            
+            #get button state
+            for i in range(0, self.joystick.get_numbuttons()):
+                if self.joystick.get_button(i) != 0:
+                    tmp = "self.cmd.btn" + str(i) + " = 1"
+                    print(tmp)
+                    exec(tmp)
+
+            if self.cmd.btn0:
+                #toggle IO
+                air = not air
+                self.robot.set_digital_out(2, air)
+
+            if self.cmd.btn9:
+                print("moving to init pose")
+                self.robot.movej(self.init_pose, acc=1, vel=0.1)
+            
+            #initalize speed array to 0
+            speeds = [0, 0, 0, 0, 0, 0]
+
+            #get linear speed from joystick
+            speeds[0] = self.cmd.axis0 * self.linear_velocity
+            speeds[1] = self.cmd.axis1 * self.linear_velocity
+            if self.cmd.btn4 and not self.cmd.btn6:
+                speeds[2] = self.linear_velocity
+            if self.cmd.btn6 and not self.cmd.btn4:
+                speeds[2] = -self.linear_velocity
+
+            #get rotational speed from joystick
+            speeds[4] = -1 * self.cmd.axis2 * self.rotational_velocity
+            speeds[3] = self.cmd.axis3 * self.rotational_velocity
+            if self.cmd.btn1 and not self.cmd.btn2:
+                speeds[5] = self.rotational_velocity
+            if self.cmd.btn2 and not self.cmd.btn1:
+                speeds[5] = -self.rotational_velocity
+            
+            #for some reasons everything is inversed
+            speeds = [-i for i in speeds]
+            #Now sending to robot. tol by default and base csys if btn2 is on
+            if speeds != [0 for _ in speeds]:
+                print("Sending ", speeds)
+            if self.cmd.btn7:
+                self.robot.speedl_tool(speeds, acc=self.acceleration, min_time=2)
+            else:
+                self.robot.speedl(speeds, acc=self.acceleration, min_time=2)
+
+            time.sleep(0.12)#URX secondday port accepts command at 10Hz, no need to go faster
+
+    def cleanup(self):
+        if self.joystick:
+            self.joystick.quit()
+
+
+if __name__ == "__main__":
+    #create robot object 
+    #robot = urx.Robot('192.168.0.90')
+    robot = urx.Robot('localhost')
+    #set a base transformation for robot (optional)
+    robot.set_tcp((0, 0, 0, 0, 0, 0))
+    trx = m3d.Transform()
+    trx.orient.rotate_zb(-pi/4)
+    robot.set_csys("mycsys", trx)
+    
+    #start joystick service with given max speed and acceleration
+    service = Service(robot, linear_velocity=0.1, rotational_velocity=0.1, acceleration=0.1)
+    service.init_joystick()
+    try:
+        service.loop() 
+    finally: 
+        robot.cleanup()
+        service.cleanup()

examples/spnav_control.py

@@ -0,0 +1,94 @@
+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.cleanup()
+        spnav.spnav_close()
+
+
+