From 063ab4fd43d1e8c142fdac37d8adffb26f92c6f2 Mon Sep 17 00:00:00 2001 From: Michael Soukup Date: Wed, 6 Aug 2014 13:43:11 +0200 Subject: [PATCH] Added gamepad support --- config.py | 40 +++++++++++++++++++++ config.pyc | Bin 1751 -> 1983 bytes demo.py | 88 +++++++++++++++++++++++++++++++++++++++++++-- testgamepad.py | 90 ++++++++++++++++++++++++++++++++++++++++++++++ unittesting.py | 2 ++ ur5controller.py | 4 +-- ur5controller.pyc | Bin 12771 -> 12841 bytes urx/__init__.pyc | Bin 350 -> 353 bytes urx/robot.pyc | Bin 25578 -> 25788 bytes urx/urrtmon.pyc | Bin 9187 -> 9250 bytes urx/ursecmon.pyc | Bin 15814 -> 15898 bytes 11 files changed, 220 insertions(+), 4 deletions(-) create mode 100755 testgamepad.py diff --git a/config.py b/config.py index 11986df..03f93ee 100644 --- a/config.py +++ b/config.py @@ -86,6 +86,9 @@ T_FORCE_VIOLATION = 0.3 # Note that the angular velocity is also [m/s], not [rad/s], to ensure that the robot # moves with constant speed regardless of direction in the polar plane. # IMPORTANT: VEL_Z should be tuned to Z_MIN to avoid smashing into the table. +# +# T_DIR_CHANGE_COOLDOWN is a cooldown between change of commands. With this, the robot +# have a chance to decelerate before changing direction, which smooths out motion. VEL = 0.1 #[m/s] VEL_Z = 0.05 #[m/s] ACC = 0.5 #[m/s^2] @@ -106,6 +109,43 @@ Z_MAX = TABLE_Z + 4.5*BLOCK_DIM #4 blocks high +# GAMEPAD MAP +# +# For the Microsoft xBox 360 controller. Button/axes codes are listed +# below for reference. +# +# Buttons +# A:0 +# B:1 +# X:2 +# Y:3 +# LB:4 +# RB:5 +# Back:6 +# Start:7 +BUTTON_MAP = { + 4: (0, 0, -1), + 5: (0, 0, 1) +} +# Hat switch:0 (All or nothing for direction). returns (+-1, +-1) = (+-x. +-y) +# hx, hy = hat +HATCODE = 0 #set to -1 to avoid polling hat +HX = 10 +HY = 11 +HAT_MAP = { + HX: (0, 1, 0), + HY: (-1, 0, 0) #inverted +} +# Axes (Not supported) +# Joystick left, x-axis:0 +# Joystick left, y-axis:1 +# Joystick right, x-axis:3 +# Joystick right, x-axis:4 +# Left trigger:2 +# Right trigger:5 + + + # AUTO/LOOP CONTROL # # When looping, the robot use the same velocity for r, theta and z. diff --git a/config.pyc b/config.pyc index c4d04e3098f7ab4160714a561ab8aa357220eef5..311f2532becbb2c6760269833567c3956323d3ae 100644 GIT binary patch delta 532 zcmZ8e$xZ@65Pi+SU?4ywB4A8#VHsS&m3S1z1WnumCR!I`tVzJI9OU55Z!q&IoQ=QW z&65`sPbOac0IeP{E=Y7Fx9cJ0q$^ z*jZ8Kz;TcTPJr^@BuHEY#bgE~+A!=OID%N5qy@N&ASm0!1}V>KqJX{O@(+=qA~>!4 z$7sp@nq?PPz!}i0o3FX{Nl~op^VY0_^QKNHdb-DkUdbH7rwr>~ zO}GB4xlX!cP}3bh23D!RX*{bny~)-|y?%IbzF$7->6@sQ>)VHwou1qUs;7f+_RVNj zf6|vyqbZ?_jc(g6wwsrY3%lj)nrU_8bj*!>$W+6}uVYXJR9Jp49Y>G^d=9OkJCG2)K99{i9o`NqIDrNpL&yPo4YGX1 z7m$-`;E1OX4}4k2E66GMy2g3EFSZ@C>dfVMWHRT=ByLIb&CdO#%}+Zab!@-MAIg=^ kMXyRT@Kd@sg};~w9yRJ$xkCXdQmWw`&sH%-Uhv;VeHq)$ diff --git a/demo.py b/demo.py index 1a5abf0..02e849b 100755 --- a/demo.py +++ b/demo.py @@ -14,6 +14,13 @@ class UR5Demo(object): pygame.display.set_caption("UR5 Demo") self.font = pygame.font.SysFont(None, config.FONTSIZE) + pygame.joystick.init() + self.padcount = pygame.joystick.get_count() + self.gamepad = None + self.hatcode = config.HATCODE + self.hat_map = config.HAT_MAP + self.but_map = config.BUTTON_MAP + # move vectors are in cylinder coords self.kb_map = { pygame.K_UP: (-1,0,0), @@ -39,11 +46,21 @@ class UR5Demo(object): msg = [] if error: msg.append(error) + + msg.append("Setting robot to freedrive mode...") + self._disptxt(msg) + try: + self.controller.set_freedrive(True) + except Exception, e: + msg.append("Could not set freedrive mode: %s" % e) + self._disptxt(msg) + time.sleep(1) # give freedrive mode some time to set + msg.append("Disconnecting robot...") self._disptxt(msg) if self.controller: self.controller.cleanup() - time.sleep(3) # Give the UR5 threads a chance to terminate + time.sleep(2) # Give the UR5 threads a chance to terminate msg.append("Exit...") self._disptxt(msg) pygame.quit() @@ -216,6 +233,47 @@ class UR5Demo(object): self.clock.tick(config.MENU_FPS) + def gamepad_control(self): + t = time.time() + running = True + while running: + pressed = pygame.key.get_pressed() + for e in pygame.event.get(): + if e.type == pygame.QUIT: + self.sig_quit = True + if pressed[pygame.K_ESCAPE] or self.sig_quit: + running = False + + vec = (0,0,0) + if self.hatcode >= 0: + hx, hy = self.gamepad.get_hat(self.hatcode) + mx = [hx*i for i in self.hat_map[config.HX]] + my = [hy*i for i in self.hat_map[config.HY]] + vec = map(sum, zip(vec, mx, my)) + for m in (self.but_map[code] for code in self.but_map if self.gamepad.get_button(code) > 0): + vec = map(sum, zip(vec, m)) + + new_t = time.time() + dt = max(new_t-t, 1/config.CTR_FPS) + t = new_t + + try: + self.controller.update(tuple(vec), dt) + except Exception, e: + self._disptxt(["Error: %s" % e]) + time.sleep(2) + running = False + + f = self.controller.zforce + self._disptxt([ + "Gamepad control \"%s\"" % self.gamepad.get_name(), + " ", + "move vec: %s" % str(vec), + "dt: %.3f" % dt, + "force z: %s %.1f" % (" " if f>0 else "", f) + ]) + self.clock.tick(config.CTR_FPS) + def keyboard_control(self): t = time.time() running = True @@ -308,7 +366,10 @@ class UR5Demo(object): self._disptxt(menu) elif pressed[pygame.K_c]: - self.keyboard_control() + if self.gamepad: + self.gamepad_control() + else: + self.keyboard_control() self._disptxt(menu) elif pressed[pygame.K_q] or self.sig_quit: @@ -325,7 +386,11 @@ class UR5Demo(object): self._disptxt(msg) try: self.controller = DemoController(config) + msg.append("Freedrive mode off...") + self._disptxt(msg) self.controller.set_freedrive(False) + time.sleep(0.5) + msg.append("Going to home position...") msg.append("Calibrating cylinder coordinate system...") self._disptxt(msg) self.controller.calibrate_cylinder_sys() @@ -333,6 +398,25 @@ class UR5Demo(object): return self.quit(error=("Error: %s" % e)) msg.append("Calibration done.") self._disptxt(msg) + + if self.padcount > 0: + msg.append("Found %d gamepad" % self.padcount) + msg.append("Initializing gamepad..") + self._disptxt(msg) + try: + self.gamepad = pygame.joystick.Joystick(0) + self.gamepad.init() + except Exception, e: + return self.quit(error=("Error: %s" % e)) + msg.append("Gamepad \"%s\" initialized" % self.gamepad.get_name()) + else: + msg.append("Found no gamepads") + msg.append("Using keyboard for manual control") + + self._disptxt(msg) + time.sleep(1) + msg.append("Starting application...") + self._disptxt(msg) time.sleep(1) return self.main_menu() diff --git a/testgamepad.py b/testgamepad.py new file mode 100755 index 0000000..a103dd5 --- /dev/null +++ b/testgamepad.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import pygame, sys + + +def main(): + pygame.init() + screen = pygame.display.set_mode((600,800)) + font = pygame.font.SysFont(None, 24) + clock = pygame.time.Clock() + + def _disptxt(s_arr): + screen.fill((255,255,255)) + for i,s in enumerate(s_arr): + screen.blit(font.render(s, True, (0,0,0)), (10, 10 + i*24)) + pygame.display.flip() + + pad_map = { + pygame.K_UP: (-1,0,0), + pygame.K_DOWN: (1,0,0), + pygame.K_LEFT: (0,-1,0), + pygame.K_RIGHT: (0,1,0), + pygame.K_w: (0,0,1), + pygame.K_s: (0,0,-1) + } + + pygame.joystick.init() + joystick_count = pygame.joystick.get_count() + if joystick_count > 0: + _disptxt(["Found %d joysticks." % joystick_count]) + else: + _disptxt(["Found no joysticks.", "Exit..."]) + time.sleep(1) + pygame.quit() + return 1 + + joystick = pygame.joystick.Joystick(0) + joystick.init() + axes = joystick.get_numaxes() + buttons = joystick.get_numbuttons() + hats = joystick.get_numhats() + _disptxt([ + "Using joystick \"%s\"" % joystick.get_name(), + "Number of axes: %d" % axes, + "Number of buttons: %d" % buttons, + "Number of hats: %d" % hats + + ]) + + + running = True + while running: + b_msg = [] + ax_msg = [] + hat_msg = [] + for e in pygame.event.get(): + pressed = pygame.key.get_pressed() + if e.type == pygame.QUIT or pressed[pygame.K_ESCAPE]: + running = False + + # Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION + #if e.type == pygame.JOYBUTTONDOWN: + # b_msg.append("Joystick button pressed.") + #if e.type == pygame.JOYBUTTONUP: + # b_msg.append("Joystick button released.") + + + for i in range(axes): + axis = joystick.get_axis(i) + ax_msg.append("Axis %d value %.2f" % (i, axis)) + + for i in range(buttons): + button = joystick.get_button(i) + b_msg.append("Button %d value %.2f" % (i, button)) + + # Hat switch. All or nothing for direction, not like joysticks. + # Value comes back in an array. + for i in range(hats): + hat = joystick.get_hat(i) + hat_msg.append("Hat %d value %s" % (i, str(hat))) + + _disptxt(b_msg + hat_msg + ax_msg) + clock.tick(60) + + pygame.quit() + + +if __name__ == '__main__': + main() diff --git a/unittesting.py b/unittesting.py index eb94b3a..12ad9c8 100755 --- a/unittesting.py +++ b/unittesting.py @@ -314,6 +314,8 @@ if __name__ == '__main__': print "Initializing..." try: controller = DemoController(config) + controller.set_freedrive(False) + time.sleep(1) controller.calibrate_cylinder_sys() except Exception, e: print e diff --git a/ur5controller.py b/ur5controller.py index ea857b4..56cda5e 100644 --- a/ur5controller.py +++ b/ur5controller.py @@ -250,7 +250,7 @@ class DemoController(object): # check projected constraints. rnext = r + dr*0.031 thetanext = theta + dtheta*0.029 - znext = z + dz*0.012 + znext = z + dz*0.009 if (rnext < self.r_min and dr < 0) or (rnext > self.r_max and dr > 0): r = self.r_min if dr < 0 else self.r_max dr = 0 @@ -277,7 +277,7 @@ class DemoController(object): self.t_ch = 0 elif not self.t_ch: #from another command self.t_ch = time.time() - self.robot.stopl(acc=self.stop_acc) + self.robot.stopl(acc=self.stop_acc*2) elif time.time() - self.t_ch > self.t_chcmd: self.move(vec) self.prev_vec = vec diff --git a/ur5controller.pyc b/ur5controller.pyc index 7b38aaa84bd1677ae7e8679d76ab61e9be13808b..7a34ed56814d69596e6dc5dfe943394aacb05f51 100644 GIT binary patch delta 667 zcmaEyyfTHI`7Z_mGZIsCCU4Z2+APL+odcT`uheEy zu0Cc=sm&XCH!@-hPu>XB(Z%nEOX`Qf3|vw>gu-!2nTzbjB_$(Pz=X{iKtFAf*o?*D zU>Ekw_~SB=O>QL)11DcsaAd6AEU9>eiLr9>J!N~YO6D2{hAc6LVvEV@DoR`_ObjB; z42+D0m6QEdq=9PwC@TWVUX=_E>-+w_)AO3_Ckv{IZWdDI#9{knXSH-j+0Cofe&SMa zM&lb6?=tGkZGNZehQqs)V|1@!(FBaD$x(VUvB)s$FTx_aQNIzZD9{vx6*y$H3@fn7 K0Bt^HCB4$iOu delta 622 zcmZ3P@;I5D`7FkFS*`M<%GcYjdXXNLm>ZhdU=1+d8AhlVN@j3^Z_-00~70hU2 zlNq_BHecbL&WKHX8owJ>adyEOSjA5Xg<}=>5!s7PeDVZcmdzGoIZWtwgWP{!Vl!5^ zu9fk}X4YnPxs_PViDs}gIa0p`T^JaIlQ|61u?f#I USbFkFa*~J<8lo%Ko^fU5vQ}t6)bMq(bGa3N^%1R25 diff --git a/urx/robot.pyc b/urx/robot.pyc index 5acd80f52faac06ed42d98fada19149106f86ec2..673d0eef532fdf802e2da89f04008d502065b84b 100644 GIT binary patch delta 1866 zcmZ|PT}YE*6bJCFt^8_u@_kDkSu)kTcXE=W6 zWV^=J<}w~RRH@BotLPr+@2u$WvAW}(S12Ac^pYdB15m;;DVhCQkjb4*fG)F-J4kSthy(KVxCOubAQ4?ig{MC=(as>T=Fdl z@UR_uQ-W0I-xuU({)8a9U|Nt76Ahz6Ub<2E9b|Z^w~E&BFou5My|xXc_z9{id4*C; zS$i)?bi2$z#BD74ZUWK~Kl+&>{$g$4??&CJpsB;<|3EQfvH}mKR9R&&O7R8GS9J)B zsHuLBLVVP|1HFj^zcg{oU>}i zwu)Av7ib#3*41!Wq9aGUoLh_Wmxh*W7X*2J0*;ja1Rbku0U3Ts7U}|04_PORC2Fj1 zMZ^U^Hk?7ELZ+@g)r^RDH>)yr>U6H~b$zx0Yw|V<&aQ*WSOSr+f|NHs735dbfFOx; zSAp=rMsquwiZA$a%LpPKIOf@|>(37%=Hyy*P#_w-l+5x#-09OT4ik-KQZn+ruXv6^sDY$cjfP$*8GD834taFWDy zY{CkXi?9j5lw6HXc(>FAbm4O`IcNM6S{g({Ff^#P;vlSGr^<)!x};J?dZmUl)pD`z%I-&d7ecm zHYq8~qv*n*M8syb4qX^z{%xxT=)%DG-@MEkGmV2HF51=x!&e~jeRkN;T zO<3Ed7+o0Tn4>O{ScEsby6R%Jbf%j;R`L7pF&N@N<2NUH`k{+&KIoN)E;bpI$)kLP zv6X0xr}$$i2l;b%zz1}fgM=>y-ohq)IcPpM;hDkt*o5^%3eki&Zw#rx=8Vk&VNDp~ zAfNvX$F6=xWD$mXkh=||zoJ<=85BRhF~87-L8|j&FQEx<4vag2)nJE&rD#gQB^*!U T7c}9`f=PxLc?9H!%#>gNQ)%>A diff --git a/urx/urrtmon.pyc b/urx/urrtmon.pyc index 6ca56df9f75f6a268239ff7df0ab73afbe7874ea..fd89343e72f4225211aade13319e39f63d212c0e 100644 GIT binary patch delta 574 zcmaFtzQ}`}`7fMNZO=q2mrlHrjq~w delta 497 zcmZ4F@z|Z6`7FkFa+3h*_6d4#8^fU5vQ}t6)bMq%R@`_K+<5-3%EVJ2>GnElj zTw?Nhu0Bj*@yV^+_b`R!CQsr)7v9{+n~lX_naz^?c38y4H`fa=vZ5Kid7>yM7PS(S z4~ylZDV^*n{ufN0s)luzC#laEa}O!g%f e;msLx*nA_td8Yg?CT!~WC~;tMjLhVZ%7y?gPmu!v diff --git a/urx/ursecmon.pyc b/urx/ursecmon.pyc index 0ba9b34189670f0bc899cc6fd5e4738446deea21..ab37941698799c15c059f89a77253cddf52f3f86 100644 GIT binary patch delta 727 zcmX?BJ*$SD`7=K*Iu40#1YrYn{M1#ditP-2^Egf;V_nOsF atXd`?v|fTuLUMAC%_D3Q5|gLc8UXFkFa*+W_Qlo%Ko^fU5vQ}t6)bMq%3FqW9y%HoDi_zTM_Y{JJ` zO|S@WR$-H8#3r7@?!}KOF1h)E*mkUrIUv0SoAP-w0!*0V5}U8f9b(25mz;c9X(Fbu z#O7M%KUkDYY+j^(0*mWqH_y=gi^Y|aoA>KgU{NozIl;gOoA~6bh8r=RBRP44(SJ=ZCiTI54L0Fp7ALR>Z{A_)fWjeO1v)P>h