Added gamepad support

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.

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,6 +366,9 @@ class UR5Demo(object):
                 self._disptxt(menu)
 
             elif pressed[pygame.K_c]:
+                if self.gamepad:
+                    self.gamepad_control()
+                else:
                     self.keyboard_control()
                 self._disptxt(menu)
 
@@ -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()

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()

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

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