Added two loops: tower and spread out
This commit is contained in:
Michael Soukup
parent
c507a58a36
commit
09619236ef
222
demo.py
222
demo.py
@ -224,6 +224,192 @@ class UR5Demo(object):
|
|||||||
self.controller.robot.stopj(acc)
|
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):
|
def loop_init(self, block_init_msg, loopnum):
|
||||||
msg = [
|
msg = [
|
||||||
"Current loop speed is %.2f m/s" % self.loop_speed,
|
"Current loop speed is %.2f m/s" % self.loop_speed,
|
||||||
@ -242,6 +428,10 @@ class UR5Demo(object):
|
|||||||
self.run_loop1()
|
self.run_loop1()
|
||||||
elif loopnum == 2:
|
elif loopnum == 2:
|
||||||
self.run_loop2()
|
self.run_loop2()
|
||||||
|
elif loopnum == 3:
|
||||||
|
self.run_loop3()
|
||||||
|
elif loopnum == 4:
|
||||||
|
self.run_loop4()
|
||||||
running = False
|
running = False
|
||||||
|
|
||||||
if pressed[pygame.K_ESCAPE] or self.sig_quit:
|
if pressed[pygame.K_ESCAPE] or self.sig_quit:
|
||||||
@ -335,6 +525,8 @@ class UR5Demo(object):
|
|||||||
"Adjust loop [s]peed",
|
"Adjust loop [s]peed",
|
||||||
"Start loop [1]: four blocks high",
|
"Start loop [1]: four blocks high",
|
||||||
"Start loop [2]: small pyramid",
|
"Start loop [2]: small pyramid",
|
||||||
|
"Start loop [3]: tower",
|
||||||
|
"Start loop [4]: spread out",
|
||||||
"Manual [c]ontrol",
|
"Manual [c]ontrol",
|
||||||
"[q]uit"
|
"[q]uit"
|
||||||
]
|
]
|
||||||
@ -381,6 +573,36 @@ class UR5Demo(object):
|
|||||||
self.loop_init(_instr, 2)
|
self.loop_init(_instr, 2)
|
||||||
self._disptxt(menu)
|
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]:
|
elif pressed[pygame.K_c]:
|
||||||
if self.gamepad:
|
if self.gamepad:
|
||||||
self.gamepad_control()
|
self.gamepad_control()
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user