From 09619236ef56cda4ff6dada9126975a22d29146d Mon Sep 17 00:00:00 2001 From: Michael Soukup Date: Thu, 7 Aug 2014 14:18:35 +0200 Subject: [PATCH] Added two loops: tower and spread out --- demo.py | 222 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 222 insertions(+) diff --git a/demo.py b/demo.py index bfd5a71..90dba5c 100755 --- a/demo.py +++ b/demo.py @@ -224,6 +224,192 @@ class UR5Demo(object): self.controller.robot.stopj(acc) + def run_loop3(self): + vel = self.loop_speed + acc = 1.1*vel + dtheta = 0.5*config.BLOCK_DIM/config.R_LVL0 + math.pi/128 #small gap + edge_left = config.THETA_EDGE_LEFT + edge_right = config.THETA_EDGE_RIGHT + + moves = [] + #level 1 + moves.append(self.controller.blocklevel2move(1.5, 0, 0, acc, vel, 0)) + moves += self.controller.pick_block(1, edge_left, 2, acc, vel) + moves.append(self.controller.blocklevel2move(1.1, (edge_left-dtheta)/2, 2.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, -dtheta, 0, acc, vel) + + moves.append(self.controller.blocklevel2move(1.1, (edge_right-dtheta)/2, 2.0, acc, vel, 0.15)) + moves += self.controller.pick_block(1, edge_right, 2, acc, vel) + moves.append(self.controller.blocklevel2move(1.1, (edge_right+dtheta)/2, 2.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, dtheta, 0, acc, vel) + + #level 2 + moves.append(self.controller.blocklevel2move(1.1, (edge_left+dtheta)/2, 2.0, acc, vel, 0.15)) + moves += self.controller.pick_block(0, edge_left, 2, acc, vel) + moves.append(self.controller.blocklevel2move(0.1, (edge_left-dtheta)/2, 2.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, -dtheta, 1, acc, vel) + + moves.append(self.controller.blocklevel2move(1.1, (edge_right-dtheta)/2, 2.0, acc, vel, 0.15)) + moves += self.controller.pick_block(0, edge_right, 2, acc, vel) + moves.append(self.controller.blocklevel2move(0.1, (edge_right+dtheta)/2, 2.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, dtheta, 1, acc, vel) + + #level 3 + moves.append(self.controller.blocklevel2move(1.1, (edge_left+dtheta)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.pick_block(1, edge_left, 1, acc, vel) + moves.append(self.controller.blocklevel2move(1.1, (edge_left-dtheta)/2, 2.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, -dtheta, 2, acc, vel) + + moves.append(self.controller.blocklevel2move(1.1, (edge_right-dtheta)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.pick_block(1, edge_right, 1, acc, vel) + moves.append(self.controller.blocklevel2move(1.1, (edge_right+dtheta)/2, 2.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, dtheta, 2, acc, vel) + + #level 4 + moves.append(self.controller.blocklevel2move(1.1, (edge_left+dtheta)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.pick_block(0, edge_left, 1, acc, vel) + moves.append(self.controller.blocklevel2move(0.1, (edge_left-dtheta)/2, 3.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, -dtheta, 3, acc, vel) + + moves.append(self.controller.blocklevel2move(1.1, (edge_right-dtheta)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.pick_block(0, edge_right, 1, acc, vel) + moves.append(self.controller.blocklevel2move(0.1, (edge_right+dtheta)/2, 3.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, dtheta, 3, acc, vel) + + #level 5 + moves.append(self.controller.blocklevel2move(1.1, (edge_left+dtheta)/2, 1.0, acc, vel, 0.15)) + moves += self.controller.pick_block(1, edge_left, 0, acc, vel) + moves.append(self.controller.blocklevel2move(1.1, (edge_left-dtheta)/2, 4.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, -dtheta, 4, acc, vel) + + moves.append(self.controller.blocklevel2move(1.1, (edge_right-dtheta)/2, 1.0, acc, vel, 0.15)) + moves += self.controller.pick_block(1, edge_right, 0, acc, vel) + moves.append(self.controller.blocklevel2move(1.1, (edge_right+dtheta)/2, 4.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, dtheta, 4, acc, vel) + + #level 6 + moves.append(self.controller.blocklevel2move(1.1, (edge_left+dtheta)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.pick_block(0, edge_left, 0, acc, vel) + moves.append(self.controller.blocklevel2move(0.1, (edge_left-dtheta)/2, 5.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, -dtheta, 5, acc, vel) + + moves.append(self.controller.blocklevel2move(1.1, (edge_right-dtheta)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.pick_block(0, edge_right, 0, acc, vel) + moves.append(self.controller.blocklevel2move(0.1, (edge_right+dtheta)/2, 5.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, dtheta, 5, acc, vel) + + #end + moves.append(self.controller.blocklevel2move(1.1, 2*dtheta, 0.5, acc, vel, 0)) + + loopcount = 0.0 + self._disptxt(["Running loop \"tower\"", "Loopcount: %d" % math.floor(loopcount)]) + self.controller.exec_moves(moves, wait=False) + 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 + + if not self.controller.is_looping(1.0): + moves.reverse() + self.controller.exec_moves(moves, wait=False) # try-catch? + loopcount += 0.5 + self._disptxt(["Running loop \"tower\"", "Loopcount: %d" % math.floor(loopcount)]) + + #self.clock.tick(config.MENU_FPS) # we dont need this here, check loop blocks for 1 sec + self.controller.robot.stopj(acc) + + + def run_loop4(self): + vel = self.loop_speed + acc = 1.1*vel + nblocks = 12 + edge_left = config.THETA_EDGE_LEFT + edge_right = config.THETA_EDGE_RIGHT + theta0 = config.THETA_EDGE_LEFT + dtheta = (edge_right-edge_left) / (nblocks-1) + + moves = [] + moves.append(self.controller.blocklevel2move(1.5, 0, 0, acc, vel, 0)) + moves += self.controller.pick_block(1, edge_left, 1, acc, vel) + theta_next = theta0 + 5*dtheta + moves.append(self.controller.blocklevel2move(1.1, (edge_left+theta_next)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + moves += self.controller.pick_block(0, theta_next, 1, acc, vel) + theta_next += dtheta + moves.append(self.controller.blocklevel2move(0, theta_next, 1.8, acc, vel, 0.03)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + + moves.append(self.controller.blocklevel2move(1.8, (edge_right+theta_next)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.pick_block(1, edge_right, 1, acc, vel) + theta_next = theta0 + 7*dtheta + moves.append(self.controller.blocklevel2move(1.1, (edge_right+theta_next)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + moves += self.controller.pick_block(0, theta_next, 1, acc, vel) + theta_next -= 3*dtheta + moves.append(self.controller.blocklevel2move(0, theta_next, 1.8, acc, vel, 0.03)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + + moves.append(self.controller.blocklevel2move(1.1, (edge_left+theta_next)/2, 1.5, acc, vel, 0.15)) + moves += self.controller.pick_block(0, edge_left, 1, acc, vel) + theta_next = theta0 + 3*dtheta + moves.append(self.controller.blocklevel2move(0.1, (edge_left+theta_next)/2, 1.2, acc, vel, 0.1)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + moves += self.controller.pick_block(0, theta_next, 1, acc, vel) + theta_next += 5*dtheta + moves.append(self.controller.blocklevel2move(0, theta_next, 1.8, acc, vel, 0.03)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + + moves.append(self.controller.blocklevel2move(1.2, (edge_right+theta_next)/2, 1.5, acc, vel, 0.1)) + moves += self.controller.pick_block(0, edge_right, 1, acc, vel) + theta_next = theta0 + 9*dtheta + moves.append(self.controller.blocklevel2move(0.1, (edge_right+theta_next)/2, 1.5, acc, vel, 0.1)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + moves += self.controller.pick_block(0, theta_next, 1, acc, vel) + theta_next -= 7*dtheta + moves.append(self.controller.blocklevel2move(0, theta_next, 1.8, acc, vel, 0.03)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + + moves.append(self.controller.blocklevel2move(2.0, (edge_left+theta_next)/2, 0.0, acc, vel, 0.15)) + moves += self.controller.pick_block(1, edge_left, 0, acc, vel) + theta_next = theta0 + dtheta + moves.append(self.controller.blocklevel2move(1.2, theta_next, 0.5, acc, vel, 0.03)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + + moves.append(self.controller.blocklevel2move(1.1, (edge_right+theta_next)/2, 0.0, acc, vel, 0.15)) + theta_next += 9*dtheta + moves += self.controller.pick_block(1, edge_right, 0, acc, vel) + moves.append(self.controller.blocklevel2move(1.2, theta_next, 0.5, acc, vel, 0.03)) + moves += self.controller.place_block(0, theta_next, 0, acc, vel) + + moves.append(self.controller.blocklevel2move(1.5, 0, 0, acc, vel, 0)) + + + loopcount = 0.0 + self._disptxt(["Running loop \"spread out\"", "Loopcount: %d" % math.floor(loopcount)]) + self.controller.exec_moves(moves, wait=False) + 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 + + if not self.controller.is_looping(1.0): + moves.reverse() + self.controller.exec_moves(moves, wait=False) # try-catch? + loopcount += 0.5 + self._disptxt(["Running loop \"spread out\"", "Loopcount: %d" % math.floor(loopcount)]) + + #self.clock.tick(config.MENU_FPS) # we dont need this here, check loop blocks for 1 sec + self.controller.robot.stopj(acc) + + def loop_init(self, block_init_msg, loopnum): msg = [ "Current loop speed is %.2f m/s" % self.loop_speed, @@ -242,6 +428,10 @@ class UR5Demo(object): self.run_loop1() elif loopnum == 2: self.run_loop2() + elif loopnum == 3: + self.run_loop3() + elif loopnum == 4: + self.run_loop4() running = False if pressed[pygame.K_ESCAPE] or self.sig_quit: @@ -335,6 +525,8 @@ class UR5Demo(object): "Adjust loop [s]peed", "Start loop [1]: four blocks high", "Start loop [2]: small pyramid", + "Start loop [3]: tower", + "Start loop [4]: spread out", "Manual [c]ontrol", "[q]uit" ] @@ -381,6 +573,36 @@ class UR5Demo(object): self.loop_init(_instr, 2) self._disptxt(menu) + elif pressed[pygame.K_3]: + _instr = [ + "Initital block configuration for loop \"tower\":", + "On each side", + " (r0, edge, z0)", + " (r1, edge, z0)", + " (r0, edge, z1)", + " (r1, edge, z1)", + " (r0, edge, z2)", + " (r1, edge, z2)", + " " + ] + self.loop_init(_instr, 3) + self._disptxt(menu) + + elif pressed[pygame.K_4]: + _instr = [ + "Initital block configuration for loop \"spread out\":", + "On each side", + " (r0, edge, z0)", + " (r1, edge, z0)", + " (r0, edge, z1)", + " (r1, edge, z1)", + " (r0, edge, z2)", + " (r1, edge, z2)", + " " + ] + self.loop_init(_instr, 4) + self._disptxt(menu) + elif pressed[pygame.K_c]: if self.gamepad: self.gamepad_control()