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

unit tests and TUNING IN CONFIG

Browse Source
This commit is contained in:
Michael Soukup 2014-08-05 19:12:04 +02:00
parent 7347ec8b7f
commit fc7149240e
5 changed files with 245 additions and 159 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
config.py
View File

@ -27,8 +27,7 @@ UR5_HOSTNAME = 'ur-2012208984' #requires dns.
# BLOCK DIMENSION
#
# Blocks are cubic and the hole is centered.
BLOCK_DIM = 0.0975 #not exactly 10cm
BLOCK_DIM = 0.0965 #TUNED
# TABLE CONFIGURATION
@ -42,8 +41,8 @@ BLOCK_DIM = 0.0975 #not exactly 10cm
# along the base x-axis, quadrant 1.0 corresponds to the base y-axis, etc.
# Quardant 0 is recommended.
TABLE_QUADRANT = 0 #[0..4)
TABLE_ORIGO_OFFSET = -0.25 #offset from origo along base x-axis
TABLE_Z = -0.336 #this should be fine-tuned on each setup
TABLE_ORIGO_OFFSET = (-0.245, -0.003) #offset from origo along base x-axis
TABLE_Z = -0.3337 #this should be fine-tuned on each setup
TABLE_ARC = math.pi/2
@ -57,9 +56,9 @@ TABLE_ARC = math.pi/2
# coordinate system.
#
# Change joints 2-6 in QUADRANT_JOINTS_HOME to tune the tool orientation.
R_HOME = 0.5
R_HOME = 0.55
THETA_HOME = 0
Z_HOME = TABLE_Z + 2*BLOCK_DIM
Z_HOME = TABLE_Z + BLOCK_DIM
JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
-3*math.pi/4,
-3*math.pi/4,
@ -67,8 +66,6 @@ JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
-math.pi/2,
49*math.pi/50]
<<<<<<< HEAD
# FORCE MONITOR
#
@ -120,49 +117,10 @@ LOOP_VEL_MIN = 0.05
LOOP_VEL_MAX = 0.7
LOOP_ACC = 0.5
R_LVL0 = 0.7 #tip of tool at table edge
R_LVL0 = 0.691 #tip of tool at table edge
Z_LVL0 = TABLE_Z + BLOCK_DIM/2 #tool in middle of block
THETA_EDGE_LEFT = -math.pi/4 + math.pi/68 #empirical. table is not exact
=======
# Enables force constraints on the tool. Note: Setting this to "True" involves
# polling the UR5 through a real-time monitor that runs at 125Hz. If the controller
# runs slow, try disabling this first.
USE_FORCE_MONITOR = True
FORCE_CONSTRAINT = 60
FORCE_T = 0.1
# Control parameters.
VELOCITY = 0.1
ACCELERATION = 0.5
DECELERATION = 0.5
CHCMD_DECEL = 0.8
CHCMD_T = 0.01 # cooldown between commands to smooth out the change in motion
# Loop parameters
DEFAULT_LOOP_SPEED = 0.1
LOOP_SPEED_MAX = 0.7
LOOP_SPEED_MIN = 0.05
# Block parameters
BLOCK_DIM = 0.0975 #TODO measure
# 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
Z_TABLE = -0.336
R_MIN = 0.6 # relative to TABLE center
R_MAX = 0.9 # relative to TABLE center
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
>>>>>>> parent of ccb5ffb... manual controller done. Going to separate control parameters for manual mode and auto mode
THETA_EDGE_RIGHT = math.pi/4 - math.pi/68
THETA_EDGE_LEFT = -math.pi/4 + math.pi/74 #empirical. table is not exact
THETA_EDGE_RIGHT = math.pi/4 - math.pi/74
# Offsets when picking (and placing) blocks.
# The place-block move is the reverse of the pick-block move,

BIN
config.pyc
View File

Binary file not shown.

242
unittesting.py
View File

@ -5,66 +5,179 @@ import sys, config, math, time, traceback
from ur5controller import DemoController
def check_edges(controller):
controller.movel(config.R_MIN, config.THETA_MAX, config.Z_MIN)
print 'Right edge.'
def check_z_levels(controller):
print 'Going to home pose.'
controller.move_to_home_pose()
time.sleep(1)
r, theta, z = controller.current_cyl
print "z-level 0"
z = config.Z_LVL0
move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
controller.exec_move(move, wait=True)
controller.set_cylinder_coords()
dummy = raw_input('Press any key to continue...')
controller.movel(config.R_MIN, 0, config.Z_MIN)
print 'Middle.'
time.sleep(1)
print "z-level 1"
z += config.BLOCK_DIM
move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
controller.exec_move(move, wait=True)
controller.set_cylinder_coords()
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)
print "z-level 2"
z += config.BLOCK_DIM
move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
controller.exec_move(move, wait=True)
controller.set_cylinder_coords()
dummy = raw_input('Press any key to continue...')
print "z-level 3"
z += config.BLOCK_DIM
move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
controller.exec_move(move, wait=True)
controller.set_cylinder_coords()
dummy = raw_input('Press any key to continue...')
print "z-level 4"
z += config.BLOCK_DIM
move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
controller.exec_move(move, wait=True)
controller.set_cylinder_coords()
dummy = raw_input('Press any key to continue...')
def test_unitary_moves(controller):
print 'Going to home pose.'
controller.move_to_home_pose()
time.sleep(1)
print 'Moving (+r, 0, 0)'
controller.move((1,0,0), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (-r, 0, 0)'
controller.move((-1,0,0), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (0, +theta, 0)'
controller.move((0,1,0), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (0, -theta, 0)'
controller.move((0,-1,0), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (0, 0, +z)'
controller.move((0,0,1), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (0, 0, -z)'
controller.move((0,0,-1), t=2)
time.sleep(1)
controller.set_cylinder_coords()
def test_multi_moves(controller):
print 'Going to home pose.'
controller.move_to_home_pose()
time.sleep(1)
print 'Moving (+r, +theta, 0)'
controller.move((1,1,0), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (-r, -theta, 0)'
controller.move((-1,-1,0), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (+r, -theta, 0)'
controller.move((1,-1,0), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (+r, 0, z)'
controller.move((1,0,1), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (-r, 0, z)'
controller.move((-1,0,1), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (0, theta, -z)'
controller.move((0,1,-1), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (0, -theta, -z)'
controller.move((0,-1,-1), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (r, theta, z)'
controller.move((1,1,1), t=2)
time.sleep(1)
controller.set_cylinder_coords()
print 'Moving (-r, theta, -z)'
controller.move((-1,1,-1), t=2)
time.sleep(1)
controller.set_cylinder_coords()
def check_loop_constraints(controller):
print 'Going to home pose.'
controller.move_to_home_pose()
time.sleep(1)
controller.movec_hax(config.R_LVL0, config.THETA_EDGE_RIGHT, config.Z_LVL0, wait=True)
controller.set_cylinder_coords()
print 'Right edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])
dummy = raw_input('Press any key to continue...')
controller.movec_hax(config.R_LVL0, 0, config.Z_LVL0, wait=True)
controller.set_cylinder_coords()
print 'Middle, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])
dummy = raw_input('Press any key to continue...')
controller.movec_hax(config.R_LVL0, config.THETA_EDGE_LEFT, config.Z_LVL0, wait=True)
controller.set_cylinder_coords()
print 'Left edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])
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.'
print 'Going to home pose.'
controller.move_to_home_pose()
time.sleep(1)
controller.movec(config.R_LVL0, config.THETA_EDGE_RIGHT, config.Z_LVL0, wait=True)
controller.set_cylinder_coords()
print 'Right edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])
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)
controller.movec(config.R_LVL0, 0, config.Z_LVL0, wait=True)
controller.set_cylinder_coords()
print 'Middle, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])
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.'
controller.movec(config.R_LVL0, config.THETA_EDGE_LEFT, config.Z_LVL0, wait=True)
controller.set_cylinder_coords()
print 'Left edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])
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)
@ -77,6 +190,7 @@ def check_theta_edges(controller):
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
@ -198,26 +312,44 @@ def big_pyramid(controller):
moves += controller.place_block(0, 1.5*dtheta_lvl0, 0)
def unit_tests(func_list, controller):
running = True
while running:
for i,tup in enumerate(func_list):
print '(%d) %s' % (i, tup[0])
c = raw_input('Select test or type q to quit... ')
if c == 'q':
running = False
else:
try:
idx = int(c)
f = func_list[idx][1]
f(controller)
except Exception, e:
print e
print traceback.format_exc()
running = False
if __name__ == '__main__':
controller = None
try:
controller = DemoController(config)
controller.calibrate_csys()
#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)
small_pyramid(controller)
# NEXT: test radial params and arbitrary paths.
controller.calibrate_cylinder_sys()
except Exception, e:
print e
print traceback.format_exc()
tests = [
("Check z-levels", check_z_levels),
("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)
]
unit_tests(tests, controller)
time.sleep(1)
if controller:
controller.cleanup()
sys.exit(1)
sys.exit(0)

104
ur5controller.py
View File

@ -24,8 +24,8 @@ class DemoController(object):
self.quadrant = config.TABLE_QUADRANT
self.phi = self.quadrant*math.pi/2
self.cyl_offset = config.TABLE_ORIGO_OFFSET
self.cyl_ox = self.cyl_offset*math.cos(self.phi)
self.cyl_oy = self.cyl_offset*math.sin(self.phi)
self.cyl_ox = self.cyl_offset[0]*math.cos(self.phi)
self.cyl_oy = self.cyl_offset[1]*math.sin(self.phi)
# Home pose params
self.j_home = config.JOINTS_HOME
@ -72,12 +72,12 @@ class DemoController(object):
def move_to_home_pose(self):
"""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)
self.robot.movej(self.j_home, acc=0.5, vel=0.4, radius=0, wait=True, relative=False)
pose = self.robot.getl()
pose[0] = self.r_home*math.cos(self.phi+self.theta_home) + self.cyl_ox
pose[1] = self.r_home*math.sin(self.phi+self.theta_home) + self.cyl_oy
pose[2] = self.z_home
self.robot.movel(pose, acc=0.1, vel=0.2, radius=0, wait=True, relative=False)
self.robot.movel(pose, acc=0.15, vel=0.2, radius=0, wait=True, relative=False)
self.current_cyl = (self.r_home, self.theta_home, self.z_home)
if self.debug:
print "Initial cylinder coords: %s" % str([('%.2f' % i) for i in self.current_cyl])
@ -94,8 +94,7 @@ class DemoController(object):
csys.orient.rotate_zb(self.phi)
self.robot.set_csys("csys", csys)
self.trans_base = self.robot.get_transform()
self.cyl_ox = self.cyl_offset
self.cyl_oy = 0
self.cyl_ox, self.cyl_oy = self.cyl_offset
def set_cylinder_coords(self):
@ -175,7 +174,7 @@ class DemoController(object):
return move_list
def movec_hax(self, r, theta, z, blend_last=0, wait=False):
def movec_hax(self, r, theta, z, wait=False):
"""movec with linearized arc."""
# manual acc/vel
move_list = self.linearize_arc(r, theta, z)
@ -188,43 +187,43 @@ class DemoController(object):
def move(self, vec, t=0):
"""Move in vec direction. Assumes that |v| != 0"""
vr, vtheta, vz = vec
dr, dtheta, dz = vec
r, theta, z = self.current_cyl
if vtheta == 0:
# linear move
if vz == 0:
move =
if dtheta == 0: #linear move
if dz == 0:
move = self.cylinder2cartesian(r+dr, theta, z) + [self.acc, self.vel, 0]
else:
if dr == 0:
move = self.cylinder2cartesian(r, theta, z+dz) + [self.acc, self.vel_z, 0]
else:
move = self.cylinder2cartesian(r+dr, theta, z+dz*(self.vel_z/self.vel)) + [self.acc, self.vel, 0]
self.exec_move(move)
else:
# arc move
else: #arc move
dtheta = dtheta/r # angle of arc segment with length 1
if dz == 0:
moves = self.linearize_arc(r+dr, theta+dtheta, z)
else:
moves = self.linearize_arc(r+dr, theta+dtheta, z+dz*(self.vel_z/self.vel))
self.exec_moves(moves)
if vr != 0 or (vr != 0 and vtheta != 0):
if vr != 0:
self.movel(r+vr, theta, z, wait=False)
elif vtheta != 0:
self.movec_hax(r, theta+vtheta, z, wait=False)
elif vz != 0:
self.movel(r, theta, z+vz, wait=False)
if t:
if t != 0:
time.sleep(t)
self.stopl(acc=self.chcmd_decel)
self.robot.stopl(acc=self.stop_acc)
def update(self, vec, dt):
"""Update movements based on vec (and dt?)"""
force = 10 # dummy
if self.force_mon and force > self.force_constraint:
self.move([i*-1 for i in vec], t=self.ft)
self.move([-1*i for i in vec], t=self.t_force)
self.set_current_cyl()
return
self.set_current_cyl()
vr, vtheta, vz = vec
dr, dtheta, dz = vec
r, theta, z = self.current_cyl
# move?
@ -234,30 +233,27 @@ class DemoController(object):
prev_vec = (0,0,0)
return
# check bounds
rnext = r + (vr*self.vel*dt)
# check projected constraints
rnext = r + (dr*self.vel*dt)
if rnext < self.r_min or rnext > self.r_max:
vr = 0
thetanext = theta + (vtheta*self.vel*dt) # this is angular speed, but the diff is not significant
dr = 0
thetanext = theta + (dtheta*self.vel*dt) # this is angular speed, but the diff is not significant
if thetanext < self.theta_min or thetanext > self.theta_max:
vtheta = 0
znext = z + (vz*self.vel*dt)
dtheta = 0
znext = z + (dz*self.vel_z*dt)
if znext < self.z_min or znext > self.z_max:
vz = 0
vec = (vr, vtheta, vz)
dz = 0
vec = (dr, dtheta, dz)
# command change
# change move
if vec != self.prev_vec:
# from stand still
if sum(map(abs, self.prev_vec)) == 0:
if sum(map(abs, self.prev_vec)) == 0: #from still
self.move(vec)
self.prev_vec = vec
self.t_ch = 0
# from another command
elif not self.t_ch:
elif not self.t_ch: #from another command
self.t_ch = time.time()
self.robot.stopl(acc=self.chcmd_decel)
self.robot.stopl(acc=self.stop_acc)
elif time.time() - self.t_ch > self.t_chcmd:
self.move(vec)
self.prev_vec = vec
@ -278,8 +274,10 @@ class DemoController(object):
def blocklevel2move(self, r_lvl, theta, z_lvl, acc=0.5, vel=0.1, r=0):
# loop acc/vel
return self.cylinder2pose(self.r_lvl0 - r_lvl*self.block_dim, theta, self.z_lvl0 + z_lvl*self.block_dim)
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]
return move
def pick_block(self, r_lvl, theta, z_lvl, acc, vel):
@ -295,12 +293,11 @@ class DemoController(object):
r_lvl starts at 0, which correspond to the outer edge of the table.
z_lvl starts at 0, which corrsepond to table level.
"""
# loop acc/vel
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)
r = self.r_lvl0 - r_lvl*self.block_dim
z = self.z_lvl0 + z_lvl*self.block_dim
p1 = self.cylinder2cartesian(r+self.r_pick_init, theta+self.theta_pick_init, z+self.z_pick_init)
p2 = self.cylinder2cartesian(r, theta, z)
p3 = self.cylinder2cartesian(r+self.r_pick_end, theta+self.theta_pick_end, z+self.z_pick_end)
move_list = [
p1 + [self.acc, vel, 0.005],
p2 + [self.acc, vel, 0],
@ -311,8 +308,7 @@ class DemoController(object):
def place_block(self, r_lvl, theta, z_lvl, acc, vel):
"""Reverse move of pick_block."""
# loop acc/vel
moves = self.pick_block(r_lvl, theta, z_lvl, vel)
moves = self.pick_block(r_lvl, theta, z_lvl, acc, vel)
moves.reverse()
return moves

BIN
ur5controller.pyc
View File

Binary file not shown.