Application skeleton. Added controller.is_looping() to enable non-blocking loop programs

config.py

@@ -2,6 +2,14 @@ import math
 
 DEBUG = True
 
+# GUI
+SCREEN_DIM = (800, 600)
+FONTSIZE = 24
+BG_COL = (255,255,255) #white
+FONT_COL = (0,0,0) #black
+MENU_FPS = 15 # enough to poll keys efficiently
+
+
 # IP of the robot.
 UR5_IP = "129.241.187.119"
 
@@ -15,6 +23,8 @@ VELOCITY = 0.1
 ACCELERATION = 0.5
 DECELERATION = 0.5
 DEFAULT_LOOP_SPEED = 0.1
+LOOP_SPEED_MAX = 0.7
+LOOP_SPEED_MIN = 0.05
 
 # Block parameters
 BLOCK_DIM = 0.0995 #TODO measure
@@ -43,6 +53,7 @@ THETA_BLOCKMOVE_OFFSET = 0 # theta start/end in place/pick
 Z_BLOCKMOVE_OFFSET = BLOCK_DIM*0.5 # z start/end in place/pick
 
 
+
 # Home quadrant joint configuration.
 # We need this expressed in joint space to ensure that the robot does not
 # collide with itself when moving to the base configuration.

demo.py

@@ -0,0 +1,221 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import pygame, sys, math, time, config
+from ur5controller import DemoController
+
+
+class UR5Demo(object):
+
+    def __init__(self):
+        # Set up pygame, GUI and controls
+        pygame.init()
+        self.screen = pygame.display.set_mode(config.SCREEN_DIM)
+        pygame.display.set_caption("UR5 Demo")
+        self.font = pygame.font.SysFont(None, config.FONTSIZE)
+
+        # move vectors are in cylinder coords
+        self.kb_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)
+        }
+
+        self.clock = pygame.time.Clock()
+        self.sig_quit = False
+        self.loop_speed = config.DEFAULT_LOOP_SPEED
+        self.controller = None
+
+    def _disptxt(self, s_arr):
+        self.screen.fill(config.BG_COL)
+        for i,s in enumerate(s_arr):
+            self.screen.blit(self.font.render(s, True, config.FONT_COL), (10, 10 + i*config.FONTSIZE))
+        pygame.display.flip()
+
+    def quit(self, error=None):
+        msg = []
+        if error:
+            msg.append(error)
+        msg.append("Disconnecting robot...")
+        self._disptxt(msg)
+        if self.controller:
+            self.controller.cleanup()
+        time.sleep(3) # Give the UR5 threads a chance to terminate
+        msg.append("Exit...")
+        self._disptxt(msg)
+        pygame.quit()
+        if error:
+            return 1
+        else:
+            return 0
+
+    def go_home_pos(self):
+        self._disptxt(["Going to home configuration..."])
+        self.controller.movel(config.R_MIN, 0, config.Z_MIN)
+
+    def freedrive(self):
+        self.controller.set_freedrive(True)
+        self._disptxt(["Freedrive mode"])
+        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
+
+            self.clock.tick(config.MENU_FPS)
+        self.controller.set_freedrive(False)
+
+    def adjust_loop_speed(self):
+        incr = 0.05
+        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_PLUS] or pressed[pygame.K_KP_PLUS]:
+                self.loop_speed += incr
+                self.loop_speed = min(self.loop_speed, config.LOOP_SPEED_MAX)
+            elif pressed[pygame.K_MINUS] or pressed[pygame.K_KP_MINUS]:
+                self.loop_speed -= incr
+                self.loop_speed = max(self.loop_speed, config.LOOP_SPEED_MIN)
+            elif pressed[pygame.K_ESCAPE] or self.sig_quit:
+                running = False
+
+            self._disptxt([
+                "Current speed: %.2f m/s" % self.loop_speed,
+                "(+) Increase speed",
+                "(-) Decrease speed"
+            ])
+            self.clock.tick(config.MENU_FPS)
+
+    def run_loop1(self):
+        loopcount = 0
+        self._disptxt(["Running loop 1", "Loopcount: %d" % loopcount])
+        time.sleep(1)
+        
+        # initiate loop, poll for robot move change and keypress
+        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
+            self.clock.tick(config.MENU_FPS)
+
+        # Set to default pos before return?
+
+
+    def run_loop2(self):
+        loopcount = 0
+        self._disptxt(["Running loop 2", "Loopcount: %d" % loopcount])
+        time.sleep(1)
+        pass
+
+
+    def loop_init(self, block_init_msg, loopnum):
+        msg = [
+            "Initital block configuration for loop %d:" % loopnum,
+            block_init_msg,
+            " ",
+            "Current loop speed is %.2f m/s" % self.loop_speed,
+            "Start loop? [ENTER]"
+        ]
+        self._disptxt(msg)
+        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_RETURN]:
+                if loopnum == 1:
+                    self.run_loop1()
+                elif loopnum == 2:
+                    self.run_loop2()
+                running = False
+
+            if pressed[pygame.K_ESCAPE] or self.sig_quit:
+                running = False
+
+            self.clock.tick(config.MENU_FPS)
+
+    def main_menu(self):
+        menu = [
+            "Go to [h]ome position",
+            "[f]reedrive mode",
+            "Adjust loop [s]peed",
+            "Start loop [1]",
+            "Start loop [2]",
+            "Manual [c]ontrol",
+            "[q]uit"
+        ]
+        self._disptxt(menu)
+        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_h]:
+                self.go_home_pos()
+                self._disptxt(menu)
+
+            elif pressed[pygame.K_f]:
+                self.freedrive()
+                self._disptxt(menu)
+
+            elif pressed[pygame.K_s]:
+                self.adjust_loop_speed()
+                self._disptxt(menu)
+
+            elif pressed[pygame.K_1]:
+                _instr = "2x2 blocks on left side"
+                self.loop_init(_instr, 1)
+                self._disptxt(menu)
+
+            elif pressed[pygame.K_2]:
+                _instr = "2x2 blocks on left side, 2x2 blocks on right side"
+                self.loop_init(_instr, 2)
+                self._disptxt(menu)
+
+            elif pressed[pygame.K_q] or self.sig_quit:
+                running = False
+
+            self.clock.tick(config.MENU_FPS)
+
+        return self.quit()
+        # break loop on quit, or return quit() on error
+
+    def run(self):
+        msg = []
+        msg.append("Connecting robot...")
+        self._disptxt(msg)
+        try:
+            self.controller = DemoController(config)
+            self.controller.set_freedrive(False)
+            msg.append("Calibrating cylinder coordinates...")
+            self._disptxt(msg)
+            self.controller.calibrate_csys()
+        except Exception, e:
+            return self.quit(error=("Error: %s" % e))
+        msg.append("Calibration done.")
+        self._disptxt(msg)
+        time.sleep(1)
+
+        return self.main_menu()
+
+
+if __name__ == '__main__':
+    demo = UR5Demo()
+    sys.exit(demo.run())

unittesting.py

@@ -84,24 +84,30 @@ def test_pick_place(controller):
 
     # Initial: 1x2 left side
     #speed = 0.7
-    speed = 0.05
+    speed = 0.2
 
     moves = []
     moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed)
     moves += controller.place_block(0, dtheta_lvl0/2, 0, speed)
     moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 0, speed)
     moves += controller.place_block(0, -dtheta_lvl0/2, 0, speed)
-    controller.movels(moves, wait=True)
+    controller.movels(moves, wait=False)
+    
+    looping = True
+    while looping:
+        looping = controller.is_looping(1.0) # blocks for 1 sec
+
     moves.reverse()
     controller.movels(moves, wait=True)
     # Shit is smooth!
 
 
-def pick_place_small_pyramid(controller):
+def small_pyramid(controller):
     dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap
     #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 #small gap
     #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 - math.pi/200#no gap
 
+    speed = 0.2
     # Initial: 2x2 on each side
 
 
@@ -132,6 +138,7 @@ if __name__ == '__main__':
     controller = None
     try:
         controller = DemoController(config)
+        controller.calibrate_csys()
         #controller.movel(config.R_LVL0, 0, config.Z_MIN + config.BLOCK_DIM)
         #check_edges(controller)
         #test_movec_hax(controller)

ur5controller.py

@@ -1,8 +1,10 @@
-import urx, math, math3d
+import urx, math, math3d, time
+
 
 class UR5Exception(Exception):
 	pass
 
+
 class DemoController(object):
 	"""Implements control of the UR5 in cylinder coordinates (corresponding to attached table).
 	   
@@ -51,9 +53,11 @@ class DemoController(object):
 		# Home quadrant joint config
 		self.j_home = config.QUADRANT_JOINT_CONFIG
 	
+
+	def calibrate_csys(self):
+		"""Calibrate the reference cylinder coordinate system."""
 		# Move to reference coordinate system base
 		self.set_pose_home()
-
 		# Set reference coordinate system parameters
 		csys = math3d.Transform()
 		csys.orient.rotate_zb(self.phi)
@@ -63,6 +67,11 @@ class DemoController(object):
 		self.cyl_oy = 0
 
 
+	def set_freedrive(self, mode):
+		"""Set UR5 to freedrive mode."""
+		self.robot.set_freedrive(mode)
+
+
 	def set_pose_home(self):
 		"""Move to "home" configuration. The resulting pose has correct orientation."""
 		try:
@@ -86,6 +95,7 @@ class DemoController(object):
 								  z)
 		return trans.pose_vector.tolist()
 
+
 	def blocklvl2pose(self, r_lvl, theta, z_lvl):
 		return self.cylinder2pose(self.r_lvl0 - r_lvl*self.block_dim, theta, self.z_lvl0 + z_lvl*self.block_dim)
 
@@ -135,7 +145,6 @@ class DemoController(object):
 		IMPORTANT: blending radius have to be smaller than the angular resolution! If not,
 		the robot will not finish the last move, because it is within the target
 		"""
-
 		move_list = []
 		step = math.pi/41
 		curr_r, curr_theta, curr_z = self.current_cyl
@@ -150,7 +159,6 @@ class DemoController(object):
 			move_list.append(pose + [self.acc, self.vel, 0.03])
 		move_list.append(self.cylinder2pose(r, theta, z) + [self.acc, self.vel, 0])
 
-		#debug
 		if self.debug:
 			print "move list for movec_hax"
 			for p in move_list:
@@ -178,7 +186,6 @@ class DemoController(object):
 		p1 = self.cylinder2pose(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin)
 		p2 = self.cylinder2pose(r_target, theta, z_target)
 		p3 = self.cylinder2pose(r_target + self.r_boffset, theta + self.theta_boffset, z_target + self.z_boffset)
-		# TODO take speed as arg
 		move_list = [
 			p1 + [self.acc, speed, 0.005],
 			p2 + [self.acc, speed, 0],
@@ -188,18 +195,26 @@ class DemoController(object):
 
 	def place_block(self, r_lvl, theta, z_lvl, speed):
 		"""Reverse move of pick_block."""
-		# TODO take speed as arg
 		moves = self.pick_block(r_lvl, theta, z_lvl, speed)
 		moves.reverse()
 		return moves
 
-		#r_target = self.r_lvl0 + r_lvl*self.block_dim
-		#_target = self.z_lvl0 + z_lvl*self.block_dim
 
-		#self.movec_hax(r_target + self.r_boffset, theta + self.theta_boffset, z_target + self.z_boffset)
+	def construct_moves(self, task_list, speed):
-		#self.movel(r_target, theta, z_target)
+		"""Construct moves from a list of tasks"""
-		#self.movel(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin)
+		pass # wait with implementing this
 
 
+	def is_looping(self, dt, threshold=0.001):
+		"""Polls the robot for change in pose vector. Blocks for dt seconds."""
+		pose0 = self.robot.getl()
+		time.sleep(dt)
+		pose1 = self.robot.getl()
+		v = [pose1[i]-pose0[i] for i in range(len(pose0))]
+		if self.debug:
+			_sum = sum(map(abs, v))
+			print "Velocity vector, dt=%.2f: %s. SUM %.3f" % (dt, str([('%.2f' % i) for i in v]), _sum)
+		return sum(map(abs, v)) > threshold
+
 	def cleanup(self):
 		self.robot.cleanup()