From 50b7f1a1ef1bc021e4f5f71e79c45898ae484a7d Mon Sep 17 00:00:00 2001 From: Michael Soukup Date: Mon, 4 Aug 2014 18:42:12 +0200 Subject: [PATCH] Application skeleton. Added controller.is_looping() to enable non-blocking loop programs --- config.py | 11 +++ config.pyc | Bin 1182 -> 1421 bytes demo.py | 221 ++++++++++++++++++++++++++++++++++++++++++++++ unittesting.py | 13 ++- ur5controller.py | 39 +++++--- ur5controller.pyc | Bin 8177 -> 9164 bytes 6 files changed, 269 insertions(+), 15 deletions(-) create mode 100755 demo.py 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 5adbe5428ff4e535dfbe355d156992d8f50395a8..ad1357ee2b8bb2d23db7ee3b40b39da7758e6027 100644 GIT binary patch delta 756 zcmZ8eJ#W)c6g^HHCvm=-#%Vs<(gH1Hr3^?cfi_MQsc|CQO4VH~#5JiaSine#B4uG^ z%2b5dS;~Nf#MD2*j#&5uoh!J0aa+o@eSFXHedpfy{3-iBH%Xor%#X)E-6x16E$;{N zynL$xEVqVd00F)RN`RZ7Bv=RO;6+dhyaY19X;2!R0cBKW7Mul{U=x&6m{&>xoC6iX zc~A*FDS+m{2B-`!g66>`Pz5{(vcP3f6+UpVfE>@GO0}3NQiorGFUu>a)~Er$0k$H+ zs!>z*S&TGV`qyIS#?@%aGA1z<@5>K$x~6RNsL_fVZdKuR@B-+D%5SO)6Bez(FUJk6 zXc9i3(Taw=%FeeUWwW%KQ%htgYLgZ>P#w!@tVieWwwC2b1b-x)U^OY}UP7R*#0ca; zP!gCPYPXwDRs3#`KO9BzxjpWFJ3agH`g?o&MY}r*I#M`3w2phf-n=^r&crpi6B%p> zw8R*fJzCOC8ef5#CQ{rt%38MsLS5kb<8WUkJ1<`OVM>&ay}8>9P08G?N49;>@jCrM zm=WF`Iy-Kk?9dRh+4HuB+ZXx;8Q9LQ*B!ajjl}+XQ3|(*!;$BXY`fzP9`1$zOMU0- ZQQ|~vj-)i7Fp`EAU%HW+u_^2K{{rLRj$8l$ delta 470 zcmY*VxlY4C5Pe=>@fGK=al+jo(bFL$BZMd^a5pVutRT8HNEG>kNC;O@&{HEJ(a`Y+ z^mLRosS)fh2Eu4}cINHO+qW|fZoP>)i~Ot0*WwtBx$@kVdwgU7ESG^ZfPm5f39tZ} zI<~+j$Oc;=M|)QzEwBypzz)a?8=gcdVJSpQPSkV%v*s_VY%b*fZ%2Z(lG9jK!z?yaIo;8xw8L>+KN}m-!Nct1 z<@_-p$qUq$*vhICZEyK#UqaQ*wX(as-VsA%t)7bY`dw7_7v_@@?#Y5sIHqGeW?k6Z EUn@*hlmGw# 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 a6e1091dded9eb0b6b3e3a75f790f937d12f4cbf..035e720266f69433e641b2281638b1edbf78a03c 100644 GIT binary patch delta 2211 zcmZWqUrbw77(eIUw)B?L0y4&*0oN#c7fHi#E^dmr4P=Rc?65ydHoDS#TH2Mi-g{}p zfN0qEU`)2Nz-Hn8`kn8d^Zh;F zulL4&nCvzGYDql)#V>Ptw*4{b`+0i&OE-I+BaFG6RZKQ-GS^@-!U~+ZCcDDeCS$M6 zN0}R8GN!fB9c|Oi<)8U=b%IayxJ}T`Tg;7XA;Dxbb1im-s}FhSfSc6XG;>?D(8}DD z7TTDYbJFT-{%INkMd>+-plnPjgjALVrm77o>dcA4vsNy(8ud|lkp3pKSZD=9MTt=ar`^nji4mR(v`XyR1r-eX` z#K#k&izLyEWh0*H6+KuU0Q3_$h+2#RPO2Xd9NkEfn9o&8i(W2}S=gh#F&}DXB+Mj# zi2mAnlIL;qzenfXL5eWva+n9DrUOKEWBvSz%YS>YZnH9l)P-EQ4tGjHFbtx zPa%JWVj;#35?CD2q4;*gZ0bkGHXp&D)qcOQ}_2XtjY+C z90nW#JPkMocoqP+j&7Isy$GG|MP8``qr6J9Imtg(-FqoLgKGn9y_kJqg4->zY)Ev1ochRVE7zP!$+yo1}K$MOaZ+ zgW9}i;x+w`U+iM*v?gYYvTjo2t7~DyV6Tp|^_ZrH81p_y@If4nT#JG0=;%68A9?32 zNWd`Z$1k?Rn5(UHE|85I*tpij3MT8EJVo93znL|IT?Ut9e*EJEv$aZP!?%MaX?wNB zT3{Dyo}FLHRg2OtRc(y6O;?nrHDjNs)bf{oy~7P`UlQHBex~^M?R!5>FV2qnkCDA| zk~Wos)$Kz=wi}ENW=0Cf>_I^uk|R*5Avyx_FJ0Au;K})C+j&w+DGP+s% zH>;t&XUQ^tybe;Oi8}mjrBM6B^4kfR5#wpz!qdhfZt;D5L=E;I-jAE9?_9nj zbJdl)!$~2=KoM=XNW4o=$kE(sS6}t_OoFw=6aXA9K%LOI&Vo z9q<9*mP!sxj)JOV(U@T!w33!-r7X*eSWQ+c*=jv(_0T#U@&j&kNV^k|ZUB<%>cCQD I*yuF>1q}FyI{*Lx delta 1388 zcmZ9L&u>&!6vxkfZ+^Y$n|U*ZmNM9(Ei7IQDn%M;p!64}MauwpiV3AU9p^pl3`0B4 zdo+R&BV~h;1h|O_B(7ZG!rjCe7y1V%?pR_3V_di}F)m!{`QE{{bSC%nzW3Z8=iGDd zeK`Hs*e>($RI+~imtuvU{Y?Ija|`C~>~dc;$T#V{AkU;5L~BH6=Og4r$d77kbln=e zbNP`7RYAOd(6`8o(+xrMN%9hyG2cO+rG-siQi~LM9a^MG7F}EYD!y_e>^9163hr&A z^||ptd~}tlb{L*Tc!X-BP}fYlYEtbe)y4?J+i6XZfQnIV9O3csnz3%0z#<+lP3r00 zTKA9_)gnh;jEZg?PLDE$~ z+Fs4rYzuMjiz};sO^&Ib?7qGI+?P9m7l55WFOUaz1A{7?%I`@*awC~$xF*yLL&w}$ z34Hkmhcyx%XWOK@o%(ZR0#hb|X;+ciZr~UX02X*X_y3 z?q-MT%WZw4?eSe6@>=ojp9o>AyIuS9s7(7zx+WHPQRPy-8iewMdeYSt%~r8{*6cZgY5!CUx*;|8+xnCrDn6# zlod6RlgTp}7L0}($__i>|6x{2OXUx)x1Q#HFlL`Gb30kDV(($#HDCld42%NEx!VDy zLn=Y(5-qBKdPl_t{aP;O9r1;_mj77XRR89?y0%WmqBS+RTc*Eb8Q_YN*&~wb_x^)B z^vgjGDs{iSe5v8$3~~m=&)#DBkXv9dNVtyra3FW+P1bT8(08m1rSBLjr<2%D=XG>% z0-pl6)Z>Ba11L!>8Z)e6D{Wa;#EM&KJ7e`*Im>BZjRYp^SF0acN-geLitIJI&Hn&$ CpYqiJ