michael/ur5demo
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Pick/place blocks. movec_hax works fine now.

Browse Source
This commit is contained in:
Michael Soukup 2014-08-04 15:59:36 +02:00
parent 5a0b1fbaf7
commit 0bf9fe0f2f
6 changed files with 283 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
config.py
View File

@ -14,30 +14,42 @@ USE_FORCE_MONITOR = True
VELOCITY = 0.1
ACCELERATION = 0.5
DECELERATION = 0.5
DEFAULT_LOOP_SPEED = 0.1
# Block parameters
BLOCK_DIM = 0.1 #TODO measure
BLOCK_DIM = 0.0995 #TODO measure
# Table polar parameters
TABLE_QUADRANT = 3 # In range [0..4). Quadrant 0 is along the x-axis of the robot.
# Table cylinder parameters
TABLE_QUADRANT = 0 # In range [0..4). Quadrant 0 is along the x-axis of the robot.
TABLE_ORIGO_OFFSET = -0.25 # TODO measure
TABLE_EDGE_LEFT = -math.pi/4
TABLE_EDGE_RIGHT = math.pi/4
Z_TABLE = -0.336
R_MIN = 0.6 # relative to TABLE center
R_MAX = 0.9 # relative to TABLE center
# Table height parameters (TODO CHECK THESE)
Z_TABLE = -0.337
THETA_MIN= -math.pi/4 # table edge left
THETA_MAX = math.pi/4 # table edge right
Z_MIN = Z_TABLE + BLOCK_DIM/2
Z_MAX = Z_MIN + 5*BLOCK_DIM
# Block move parameters
R_LVL0 = 0.7 # tip of tool at table edge
Z_LVL0 = Z_MIN
THETA_EDGE_LEFT = -math.pi/4 + math.pi/68
THETA_EDGE_RIGHT = math.pi/4 - math.pi/68
R_BLOCKMOVE_MARGIN = BLOCK_DIM + 0.05 # r margin before and after pick/place
THETA_BLOCKMOVE_MARGIN = 0 # theta margin before and after pick/place
Z_BLOCKMOVE_MARGIN = 0 # z margin before and after pick/place
R_BLOCKMOVE_OFFSET = -0.01 # r start/end in place/pick. lift slightly inwards to avoid collision with other blocks
THETA_BLOCKMOVE_OFFSET = 0 # theta start/end in place/pick
Z_BLOCKMOVE_OFFSET = BLOCK_DIM*0.5 # z start/end in place/pick
# Home quadrant joint configuration.
# We need this expressed in joint space to ensure that the robot does not
# collide with itself when moving to the base configuration.
# This is also for fine-tuning the tool orientation
QUADRANT_JOINT_CONFIG = [TABLE_QUADRANT*math.pi/2,
-3*math.pi/4,
-3*math.pi/4,
-math.pi/2,
-math.pi/2,
math.pi]
49*math.pi/50]

BIN
config.pyc
View File

Binary file not shown.

85
testcontroller.py
View File

@ -1,85 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys, config, math, time
from ur5controller import DemoController
def check_edges(controller):
controller.movel(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
print 'Right edge.'
dummy = raw_input('Press any key to continue...')
controller.movel(config.R_MIN, 0, config.Z_MIN)
print 'Middle.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
controller.movel(config.R_MIN, config.TABLE_EDGE_LEFT, config.Z_MIN)
print 'Left edge.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
def test_movec(controller):
controller.movec(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
print 'Right edge.'
dummy = raw_input('Press any key to continue...')
controller.movec(config.R_MIN, 0, config.Z_MIN)
print 'Middle.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
controller.movec(config.R_MIN, config.TABLE_EDGE_LEFT, config.Z_MIN)
print 'Left edge.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
def test_movec_hax(controller):
controller.movec_hax(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
print 'Right edge.'
dummy = raw_input('Press any key to continue...')
controller.movec_hax(config.R_MIN, 0, config.Z_MIN)
print 'Middle.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
controller.movec_hax(config.R_MIN, config.TABLE_EDGE_LEFT, config.Z_MIN)
print 'Left edge.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
def test_movec_hax2(controller):
controller.movec_hax(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
print 'Right edge.'
dummy = raw_input('Press any key to continue...')
controller.movec_hax(config.R_MIN, 0, config.Z_MIN+0.3)
print 'Middle.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
# NOTE: current R_MIN+0.1=0.7 is where the tooltip is at the edge
controller.movec_hax(config.R_MIN+0.1, config.TABLE_EDGE_LEFT, config.Z_MIN+0.1)
print 'Left edge.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
if __name__ == '__main__':
controller = None
try:
controller = DemoController(config)
#check_edges(controller)
test_movec_hax2(controller)
# NEXT: test radial params and arbitrary paths.
except Exception, e:
print e
time.sleep(1)
if controller:
controller.cleanup()
sys.exit(1)

149
unittesting.py Executable file
View File

@ -0,0 +1,149 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys, config, math, time
from ur5controller import DemoController
def check_edges(controller):
controller.movel(config.R_MIN, config.THETA_MAX, config.Z_MIN)
print 'Right edge.'
dummy = raw_input('Press any key to continue...')
controller.movel(config.R_MIN, 0, config.Z_MIN)
print 'Middle.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
controller.movel(config.R_MIN, config.THETA_MIN, config.Z_MIN)
print 'Left edge.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
def test_movec(controller):
controller.movec(config.R_MIN, config.THETA_MAX, config.Z_MIN)
print 'Right edge.'
dummy = raw_input('Press any key to continue...')
controller.movec(config.R_MIN, 0, config.Z_MIN)
print 'Middle.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
controller.movec(config.R_MIN, config.THETA_MIN, config.Z_MIN)
print 'Left edge.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
def test_movec_hax(controller):
controller.movec_hax(config.R_MIN, config.THETA_MAX, config.Z_MIN)
print 'Right edge.'
dummy = raw_input('Press any key to continue...')
controller.movec_hax(config.R_MIN, 0, config.Z_MIN)
print 'Middle.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
controller.movec_hax(config.R_MIN, config.THETA_MIN, config.Z_MIN)
print 'Left edge.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
def test_movec_hax2(controller):
controller.movec_hax(config.R_MIN, config.THETA_MAX, config.Z_MIN)
print 'Right edge.'
dummy = raw_input('Press any key to continue...')
controller.movec_hax(config.R_MIN, 0, config.Z_MIN+0.3)
print 'Middle.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
# NOTE: current R_MIN+0.1=0.7 is where the tooltip is at the edge
controller.movec_hax(config.R_MIN+0.1, config.THETA_MIN, config.Z_MIN+0.1)
print 'Left edge.'
time.sleep(1)
dummy = raw_input('Press any key to continue...')
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.05
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=True)
moves.reverse()
controller.movels(moves, wait=True)
# Shit is smooth!
def pick_place_small_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: 2x2 on each side
controller.blocklvl2pose(r_lvl, theta, z_lvl) + [acc, vel, rad]
controller.pick_block(1, config.THETA_EDGE_LEFT, 1) # Pick left
controller.place_block(0, -0.5*dtheta_lvl0, 0)
controller.pick_block(1, config.THETA_EDGE_RIGHT, 1) # pick right
controller.place_block(0, 0.5*dtheta_lvl0, 0)
controller.pick_block(0, config.THETA_EDGE_LEFT, 1) # Pick left
controller.place_block(0, -1.5*dtheta_lvl0, 0)
controller.pick_block(0, config.THETA_EDGE_RIGHT, 1) # pick right
controller.place_block(0, 1.5*dtheta_lvl0, 0) # foundation complete
controller.pick_block(1, config.THETA_EDGE_LEFT, 0) # Pick left
controller.place_block(0, 0, 1)
controller.pick_block(1, config.THETA_EDGE_RIGHT, 0) # pick right
controller.place_block(0, dtheta_lvl0, 1)
controller.pick_block(0, config.THETA_EDGE_LEFT, 0) # Pick left
controller.place_block(0, -dtheta_lvl0, 1)
controller.pick_block(0, config.THETA_EDGE_RIGHT, 0) # pick right
controller.place_block(0, 0, 2)
if __name__ == '__main__':
controller = None
try:
controller = DemoController(config)
#controller.movel(config.R_LVL0, 0, config.Z_MIN + config.BLOCK_DIM)
#check_edges(controller)
#test_movec_hax(controller)
#check_theta_edges(controller)
test_pick_place(controller)
#pick_place_small_pyramid(controller)
# NEXT: test radial params and arbitrary paths.
except Exception, e:
print e
time.sleep(1)
if controller:
controller.cleanup()
sys.exit(1)

148
ur5controller.py
View File

@ -5,8 +5,9 @@ class UR5Exception(Exception):
class DemoController(object):
"""Implements control of the UR5 in cylinder coordinates (corresponding to attached table).
-Checks bounds of the arc section
-Methods for placing blocks
-Checks bounds of the arc section
-Methods for placing blocks
"""
def __init__(self, config):
@ -17,7 +18,12 @@ class DemoController(object):
except Exception, e:
raise UR5Exception('Robot initiation failed: %s' % str(e))
# Set cylinder parameters
# Control params
self.vel = config.VELOCITY
self.acc = config.ACCELERATION
self.dec = config.DECELERATION
# Cylinder params
self.quadrant = config.TABLE_QUADRANT
self.phi = self.quadrant*math.pi/2
self.cyl_offset = config.TABLE_ORIGO_OFFSET
@ -26,17 +32,25 @@ class DemoController(object):
self.r_min = config.R_MIN
self.r_max = config.R_MAX
self.theta_min = config.TABLE_EDGE_LEFT
self.theta_max = config.TABLE_EDGE_RIGHT
self.theta_min = config.THETA_MIN
self.theta_max = config.THETA_MAX
self.z_min = config.Z_MIN
self.z_max = config.Z_MAX
# Block params
self.block_dim = config.BLOCK_DIM
self.r_lvl0 = config.R_LVL0
self.z_lvl0 = config.Z_LVL0
self.r_bmargin = config.R_BLOCKMOVE_MARGIN
self.theta_bmargin = config.THETA_BLOCKMOVE_MARGIN
self.z_bmargin = config.Z_BLOCKMOVE_MARGIN
self.r_boffset = config.R_BLOCKMOVE_OFFSET
self.theta_boffset = config.THETA_BLOCKMOVE_OFFSET
self.z_boffset = config.Z_BLOCKMOVE_OFFSET
# Home quadrant joint config
self.j_home = config.QUADRANT_JOINT_CONFIG
self.vel = config.VELOCITY
self.acc = config.ACCELERATION
self.dec = config.DECELERATION
# Move to reference coordinate system base
self.set_pose_home()
@ -49,9 +63,8 @@ class DemoController(object):
self.cyl_oy = 0
def set_pose_home(self):
"""Move to "home" configuration. The resulting pose ensures correct orientation."""
"""Move to "home" configuration. The resulting pose has correct orientation."""
try:
self.robot.movej(self.j_home, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
pose = self.robot.getl()
@ -65,9 +78,7 @@ class DemoController(object):
def cylinder2pose(self, r, theta, z):
"""Translate table cylinder coordinates to robot cartesian pose.
(r, theta) is given in TABLE coordinates.
"""
"""Translate table cylinder coordinates to robot cartesian pose."""
trans = math3d.Transform(self.trans_base) #deep copy
trans.orient.rotate_zb(theta)
trans.pos = math3d.Vector(r*math.cos(theta) + self.cyl_ox,
@ -75,53 +86,120 @@ class DemoController(object):
z)
return trans.pose_vector.tolist()
def blocklvl2pose(self, r_lvl, theta, z_lvl):
return self.cylinder2pose(self.r_lvl0 - r_lvl*self.block_dim, theta, self.z_lvl0 + z_lvl*self.block_dim)
def movel(self, r, theta, z):
"""Linear move."""
pose = self.cylinder2pose(r, theta, z)
self.robot.movel(pose, acc=self.acc, vel=self.vel, radius=0, wait=True, relative=False)
self.current_cyl = (r, theta, z)
def movec(self, r, theta, z):
#Note: r should be the same.
"""Circular move - uses URScript built-in function.
Unfortunately, this doesn't work very well: The tool orientation at the target pose
is not right. URScript specifies that orientation in pose_via is not considered, however,
it SHOULD consider the orientation of the end pose.
"""
curr_theta = self.current_cyl[1]
pose_via = self.cylinder2pose(r, curr_theta+(theta-curr_theta)/2, z)
pose = self.cylinder2pose(r, theta, z)
self.robot.movec(pose_via, pose, acc=self.acc, vel=self.vel, radius=0, wait=True)
self.current_cyl = (r, theta, z)
def movels(self, move_list, wait=True):
"""Concatenate several movel commands and applies a blending radius.
move_list = pose + [acc, vel, radius]
"""
header = "def myProg():\n"
end = "end\n"
template = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})\n"
prog = header + ''.join([template.format(*m) for m in move_list]) + end
self.robot.send_program(prog)
if not wait:
return None
else:
self.robot.wait_for_move()
return self.robot.getl()
def movec_hax(self, r, theta, z):
"""Since movec is unreliable in reaching the end pose: linearize the arc."""
"""movec is unreliable and does not finish in the correct pose. We work around this
by linearizing the arc into segments of a predefined angular resolution, and use
movepls with blend.
IMPORTANT: blending radius have to be smaller than the angular resolution! If not,
the robot will not finish the last move, because it is within the target
"""
move_list = []
step = math.pi/41
curr_r, curr_theta, curr_z = self.current_cyl
dtheta = theta - curr_theta
if dtheta > 0:
theta_incr = math.pi/41
else:
theta_incr = -math.pi/41
if abs(dtheta) < abs(theta_incr):
self.movel(r, theta, z)
return
pose_list = []
segments = int(math.floor(dtheta/theta_incr))
r_incr = (r-curr_r)/(segments+1)
z_incr = (z-curr_z)/(segments+1)
for i in range(1, segments+1):
pose_list.append(self.cylinder2pose(curr_r + i*r_incr,
curr_theta + i*theta_incr,
curr_z + i*z_incr))
pose_list.append(self.cylinder2pose(r, theta, z))
segments = max(int(round(abs(dtheta)/step)), 1)
theta_incr = dtheta/segments
r_incr = (r-curr_r)/segments
z_incr = (z-curr_z)/segments
for i in range(1, segments):
pose = self.cylinder2pose(curr_r + i*r_incr, curr_theta + i*theta_incr, curr_z + i*z_incr)
move_list.append(pose + [self.acc, self.vel, 0.03])
move_list.append(self.cylinder2pose(r, theta, z) + [self.acc, self.vel, 0])
#debug
if self.debug:
print "pose list for movec_hax"
for p in pose_list:
print "move list for movec_hax"
for p in move_list:
print [('%.3f' % i) for i in p]
self.robot.movels(pose_list, acc=self.acc, vel=self.vel, radius=0.05, wait=True)
self.movels(move_list, wait=True)
self.current_cyl = (r, theta, z)
def pick_block(self, r_lvl, theta, z_lvl, speed):
"""This function concatenates 3 moves:
1. Move the tool in front of the block.
2. Move the tool into the hole.
3. Lift the block.
Here, we assume that the tool is nearby the block. Bringing the tool towards the block is
not the responsibility of this function.
r_lvl starts at 0, which correspond to the outer edge of the table.
z_lvl starts at 0, which corrsepond to table level.
"""
r_target = self.r_lvl0 - r_lvl*self.block_dim
z_target = self.z_lvl0 + z_lvl*self.block_dim
p1 = self.cylinder2pose(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin)
p2 = self.cylinder2pose(r_target, theta, z_target)
p3 = self.cylinder2pose(r_target + self.r_boffset, theta + self.theta_boffset, z_target + self.z_boffset)
# TODO take speed as arg
move_list = [
p1 + [self.acc, speed, 0.005],
p2 + [self.acc, speed, 0],
p3 + [self.acc, speed, 0.01],
]
return move_list
def place_block(self, r_lvl, theta, z_lvl, speed):
"""Reverse move of pick_block."""
# TODO take speed as arg
moves = self.pick_block(r_lvl, theta, z_lvl, speed)
moves.reverse()
return moves
#r_target = self.r_lvl0 + r_lvl*self.block_dim
#_target = self.z_lvl0 + z_lvl*self.block_dim
#self.movec_hax(r_target + self.r_boffset, theta + self.theta_boffset, z_target + self.z_boffset)
#self.movel(r_target, theta, z_target)
#self.movel(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin)
def cleanup(self):
self.robot.cleanup()

BIN
ur5controller.pyc
View File

Binary file not shown.