Added two loops: tower and spread out

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