diff --git a/config.py b/config.py index cc2af14..6f24a9d 100644 --- a/config.py +++ b/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. diff --git a/config.pyc b/config.pyc index 5adbe54..ad1357e 100644 Binary files a/config.pyc and b/config.pyc differ diff --git a/demo.py b/demo.py new file mode 100755 index 0000000..edd075d --- /dev/null +++ b/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()) diff --git a/unittesting.py b/unittesting.py index f198ba7..d7b4b17 100755 --- a/unittesting.py +++ b/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) diff --git a/ur5controller.py b/ur5controller.py index 7e69d20..75d9e56 100644 --- a/ur5controller.py +++ b/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). @@ -50,10 +52,12 @@ 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) @@ -61,7 +65,12 @@ class DemoController(object): self.trans_base = self.robot.get_transform() self.cyl_ox = self.cyl_offset 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.""" @@ -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) - #self.movel(r_target, theta, z_target) - #self.movel(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin) + def construct_moves(self, task_list, speed): + """Construct moves from a list of tasks""" + 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() diff --git a/ur5controller.pyc b/ur5controller.pyc index a6e1091..035e720 100644 Binary files a/ur5controller.pyc and b/ur5controller.pyc differ