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 c4d04e3..311f253 100644 Binary files a/config.pyc and b/config.pyc differ 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 7b38aaa..7a34ed5 100644 Binary files a/ur5controller.pyc and b/ur5controller.pyc differ diff --git a/urx/__init__.pyc b/urx/__init__.pyc index 27cd43a..1c330a9 100644 Binary files a/urx/__init__.pyc and b/urx/__init__.pyc differ diff --git a/urx/robot.pyc b/urx/robot.pyc index 5acd80f..673d0ee 100644 Binary files a/urx/robot.pyc and b/urx/robot.pyc differ diff --git a/urx/urrtmon.pyc b/urx/urrtmon.pyc index 6ca56df..fd89343 100644 Binary files a/urx/urrtmon.pyc and b/urx/urrtmon.pyc differ diff --git a/urx/ursecmon.pyc b/urx/ursecmon.pyc index 0ba9b34..ab37941 100644 Binary files a/urx/ursecmon.pyc and b/urx/ursecmon.pyc differ