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)
|
||||
|
||||
|
||||
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()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user