added 2 examples files for joystick and 3d mouse control
This commit is contained in:
Olivier R-D
parent
a3dc617302
commit
4b9317368d
139
examples/joystick_control.py
Normal file
139
examples/joystick_control.py
Normal file
@ -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()
|
||||
94
examples/spnav_control.py
Normal file
94
examples/spnav_control.py
Normal file
@ -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()
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user