Two loops complete and tested: build 4 stories high and small pyramid
This commit is contained in:
Michael Soukup
parent
fc7149240e
commit
5ee0353e01
188
unittesting.py
188
unittesting.py
@ -178,139 +178,117 @@ def test_movec(controller):
|
||||
|
||||
|
||||
|
||||
def four_stories(controller):
|
||||
print "Initial block placement."
|
||||
print "On each side"
|
||||
print "\t(r0, edge, z0)"
|
||||
print "\t(r1, edge, z0)"
|
||||
|
||||
def check_theta_edges(controller):
|
||||
controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN)
|
||||
controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN)
|
||||
print 'Theta edge right.'
|
||||
dummy = raw_input('Press any key to continue...')
|
||||
|
||||
controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_LEFT, config.Z_MIN)
|
||||
controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_LEFT, config.Z_MIN)
|
||||
print 'Theta edge left.'
|
||||
dummy = raw_input('Press any key to continue...')
|
||||
|
||||
|
||||
def test_pick_place(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
|
||||
|
||||
# Initial: 1x2 left side
|
||||
#speed = 0.7
|
||||
speed = 0.2
|
||||
# buffer some params
|
||||
edge_left = config.THETA_EDGE_LEFT
|
||||
edge_right = config.THETA_EDGE_RIGHT
|
||||
r_startpick = 1.3
|
||||
r_endpick = 0.1
|
||||
z_endpick = 0.5
|
||||
|
||||
vel = 0.2
|
||||
acc = 1.1*vel
|
||||
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=False)
|
||||
|
||||
|
||||
moves.append(controller.blocklevel2move(0.5, 0, 0, acc, vel, 0))
|
||||
moves += controller.pick_block(1, edge_left, 0, acc, vel)
|
||||
moves += controller.place_block(0, 0, 0, acc, vel)
|
||||
|
||||
moves += controller.pick_block(1, edge_right, 0, acc, vel)
|
||||
moves.append(controller.blocklevel2move(1.1, edge_right/2, 2, acc, vel, 0.15))
|
||||
moves += controller.place_block(0, 0, 1, acc, vel)
|
||||
|
||||
moves += controller.pick_block(0, edge_left, 0, acc, vel)
|
||||
moves.append(controller.blocklevel2move(0.1, edge_left/2, 3, acc, vel, 0.15))
|
||||
moves += controller.place_block(0, 0, 2, acc, vel)
|
||||
|
||||
moves += controller.pick_block(0, edge_right, 0, acc, vel)
|
||||
moves.append(controller.blocklevel2move(0.1, edge_right/2, 4, acc, vel, 0.15))
|
||||
moves += controller.place_block(0, 0, 3, acc, vel)
|
||||
|
||||
moves.append(controller.blocklevel2move(1.5, 0, 0.1, acc, vel, 0))
|
||||
|
||||
dummy = raw_input("Press enter to start...")
|
||||
print 'Exectuting loop'
|
||||
controller.exec_moves(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!
|
||||
controller.exec_moves(moves, wait=False)
|
||||
looping = True
|
||||
while looping:
|
||||
looping = controller.is_looping(1.0) # blocks for 1 sec
|
||||
|
||||
|
||||
def small_pyramid(controller):
|
||||
# Initial block placement.
|
||||
# On each side
|
||||
# (r0, edge, z0)
|
||||
# (r1, edge, z0)
|
||||
# (r0, edge, z1)
|
||||
print "Initial block placement."
|
||||
print "On each side"
|
||||
print "\t(r0, edge, z0)"
|
||||
print "\t(r1, edge, z0)"
|
||||
print "\t(r0, edge, z1)"
|
||||
|
||||
dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap
|
||||
speed = 0.2
|
||||
acc = config.ACCELERATION
|
||||
# buffer some params
|
||||
edge_left = config.THETA_EDGE_LEFT
|
||||
edge_right = config.THETA_EDGE_RIGHT
|
||||
r_startpick = 1.3
|
||||
r_endpick = 0.1
|
||||
z_endpick = 0.5
|
||||
|
||||
vel = 0.2
|
||||
acc = 1.1*vel
|
||||
dtheta_lvl0 = config.BLOCK_DIM/config.R_LVL0 + math.pi/64 #medium gap
|
||||
moves = []
|
||||
|
||||
# Pick left (r0, edge, z1), place in middle
|
||||
moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 1, speed)
|
||||
tmp_pose = controller.blocklvl2pose(0.2, -dtheta_lvl0, 1.1)
|
||||
moves.append(tmp_pose + [acc, speed, 0.05])
|
||||
moves += controller.place_block(0, 0, 0, speed)
|
||||
moves.append(controller.blocklevel2move(0.5, 0, 0, acc, vel, 0)) #initial
|
||||
moves += controller.pick_block(0, edge_left, 1, acc, vel)
|
||||
moves.append(controller.blocklevel2move(0.1, edge_left/2, 2, acc, vel, 0.15))
|
||||
moves += controller.place_block(0, 0, 0, acc, vel)
|
||||
|
||||
# Pick left (r1, edge, z0), place left
|
||||
moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed)
|
||||
tmp_pose = controller.blocklvl2pose(1.1, config.THETA_EDGE_LEFT+dtheta_lvl0, 0.5)
|
||||
moves.append(tmp_pose + [acc, speed, 0.05])
|
||||
moves += controller.place_block(0, -dtheta_lvl0, 0, speed)
|
||||
moves += controller.pick_block(0, edge_right, 1, acc, vel)
|
||||
moves.append(controller.blocklevel2move(0.1, (edge_right+dtheta_lvl0)/2, 2, acc, vel, 0.15))
|
||||
moves += controller.place_block(0, dtheta_lvl0, 0, acc, vel)
|
||||
|
||||
# Pick right (r0, edge, z1), place right
|
||||
tmp_pose = controller.blocklvl2pose(1.3, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.2)
|
||||
moves.append(tmp_pose + [acc, speed, 0.05])
|
||||
#moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_RIGHT - math.pi/16, 1, vel=speed, blend_last=0.03)
|
||||
moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 1, speed)
|
||||
tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.2)
|
||||
moves.append(tmp_pose + [acc, speed, 0.03])
|
||||
moves += controller.place_block(0, dtheta_lvl0, 0, speed)
|
||||
moves.append(controller.blocklevel2move(1.1, (edge_left+dtheta_lvl0)/2, 0.5, acc, vel, 0.15))
|
||||
moves += controller.pick_block(1, edge_left, 0, acc, vel)
|
||||
moves += controller.place_block(0, -dtheta_lvl0, 0, acc, vel)
|
||||
|
||||
# Pick right (r1, edge, z0), place lvl1 right
|
||||
moves += controller.pick_block(1, config.THETA_EDGE_RIGHT, 0, speed)
|
||||
tmp_pose = controller.blocklvl2pose(1.1, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.1)
|
||||
moves.append(tmp_pose + [acc, speed, 0.03])
|
||||
moves += controller.place_block(0, 0.5*dtheta_lvl0, 1, speed)
|
||||
moves.append(controller.blocklevel2move(1.1, (edge_right-dtheta_lvl0)/2, 0.5, acc, vel, 0.15))
|
||||
moves += controller.pick_block(1, edge_right, 0, acc, vel)
|
||||
moves.append(controller.blocklevel2move(1.1, edge_right/2, 2, acc, vel, 0.15))
|
||||
moves += controller.place_block(0, 0.5*dtheta_lvl0, 1, acc, vel)
|
||||
|
||||
# Pick left (r0, edge, z0), place lvl1 left
|
||||
moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_LEFT + math.pi/16, 0.1, vel=speed, blend_last=0.03)
|
||||
moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 0, speed)
|
||||
tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_LEFT+dtheta_lvl0, 1.1)
|
||||
moves.append(tmp_pose + [acc, speed, 0.03])
|
||||
moves += controller.place_block(0, -0.5*dtheta_lvl0, 1, speed)
|
||||
moves += controller.pick_block(0, edge_left, 0, acc, vel)
|
||||
moves.append(controller.blocklevel2move(0.1, edge_left/2, 2, acc, vel, 0.15))
|
||||
moves += controller.place_block(0, -0.5*dtheta_lvl0, 1, acc, vel)
|
||||
|
||||
# Pick right (r0, edge, z0), place on top
|
||||
moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_RIGHT - math.pi/16, 0, vel=speed, blend_last=0.03)
|
||||
moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 0, speed)
|
||||
tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_RIGHT-dtheta_lvl0, 2.1)
|
||||
moves.append(tmp_pose + [acc, speed, 0.03])
|
||||
moves += controller.place_block(0, 0, 2, speed)
|
||||
moves.append(controller.blocklevel2move(1.1, (edge_right-dtheta_lvl0)/2, 0.5, acc, vel, 0.15))
|
||||
moves += controller.pick_block(0, edge_right, 0, acc, vel)
|
||||
moves.append(controller.blocklevel2move(0.1, edge_right/2, 3, acc, vel, 0.15))
|
||||
moves += controller.place_block(0, 0, 2, acc, vel)
|
||||
|
||||
# Execute moves
|
||||
controller.movels(moves, wait=False)
|
||||
moves.append(controller.blocklevel2move(1.5, 0, 0.1, acc, vel, 0))
|
||||
|
||||
dummy = raw_input("Press enter to start...")
|
||||
print 'Exectuting loop'
|
||||
controller.exec_moves(moves, wait=False)
|
||||
looping = True
|
||||
while looping:
|
||||
looping = controller.is_looping(1.0) # blocks for 1 sec
|
||||
|
||||
moves.reverse()
|
||||
controller.movels(moves, wait=True)
|
||||
controller.exec_moves(moves, wait=False)
|
||||
looping = True
|
||||
while looping:
|
||||
looping = controller.is_looping(1.0) # blocks for 1 sec
|
||||
|
||||
|
||||
def big_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
|
||||
|
||||
# Initial block placement.
|
||||
# On each side
|
||||
# (r0, edge, z0)
|
||||
# (r1, edge, z0)
|
||||
# (r0, edge, z1)
|
||||
# (r1, edge, z1)
|
||||
# (r0, edge, z2)
|
||||
# (r1, edge, z2)
|
||||
|
||||
speed = 0.2
|
||||
moves = []
|
||||
|
||||
#move += controller.blocklvl2arc(0, config.THETA_EDGE_LEFT + math.pi/16, 1, vel=speed, blend_last=0.03)
|
||||
moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 1, speed) # Pick left (r0, edge, z1)
|
||||
moves += controller.place_block(0, -0.5*dtheta_lvl0, 0)
|
||||
moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed) # Pick left (r1, edge, z0)
|
||||
moves += controller.place_block(0, -1.5*dtheta_lvl0, 0)
|
||||
|
||||
moves += controller.blocklvl2arc(0.1, config.THETA_EDGE_RIGHT - math.pi/16, 1, vel=speed, blend_last=0.03)
|
||||
moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 1, speed) # Pick right (r0, edge, z1)
|
||||
moves += controller.place_block(0, 0.5*dtheta_lvl0, 0)
|
||||
moves += controller.pick_block(1, config.THETA_EDGE_RIGHT, 0, speed) # Pick right (r1, edge, z0)
|
||||
moves += controller.place_block(0, 1.5*dtheta_lvl0, 0)
|
||||
|
||||
|
||||
def unit_tests(func_list, controller):
|
||||
running = True
|
||||
@ -345,7 +323,9 @@ if __name__ == '__main__':
|
||||
("Test unitary moves.", test_unitary_moves),
|
||||
("Test multi-directional moves", test_multi_moves),
|
||||
("Check loop constraints", check_loop_constraints),
|
||||
("Test URScript's movec", test_movec)
|
||||
("Test URScript's movec", test_movec),
|
||||
("Build four stories high", four_stories),
|
||||
("Build small pyramid", small_pyramid)
|
||||
]
|
||||
unit_tests(tests, controller)
|
||||
|
||||
|
||||
@ -272,11 +272,10 @@ class DemoController(object):
|
||||
return sum(map(abs, v)) > threshold
|
||||
|
||||
|
||||
def blocklevel2move(self, r_lvl, theta, z_lvl, acc=0.5, vel=0.1, r=0):
|
||||
# loop acc/vel
|
||||
def blocklevel2move(self, r_lvl, theta, z_lvl, acc, vel, blend):
|
||||
r = self.r_lvl0 - r_lvl*self.block_dim
|
||||
z = self.z_lvl0 + z_lvl*self.block_dim
|
||||
move = self.cylinder2cartesian(r, theta, z) + [acc, vel, r]
|
||||
move = self.cylinder2cartesian(r, theta, z) + [acc, vel, blend]
|
||||
return move
|
||||
|
||||
|
||||
|
||||
Binary file not shown.
Loading…
x
Reference in New Issue
Block a user