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

restructure

Browse Source
This commit is contained in:
Michael Soukup 2014-08-05 14:22:32 +02:00
parent ccb5ffb4c7
commit 592b5963ef
4 changed files with 275 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

169
config.py
View File

@ -2,71 +2,136 @@ import math
DEBUG = True
# GUI
#
# Frame rates should be high enough to poll keys efficiently.
SCREEN_DIM = (800, 600)
FONTSIZE = 24
BG_COL = (255,255,255) #white
FONT_COL = (0,0,0) #black
MENU_FPS = 15 # enough to poll keys efficiently
CTR_FPS = 60
BG_COL = (255,255,255)
FONT_COL = (0,0,0)
MENU_FPS = 15
CTR_FPS = 60 #manual control
# IP of the robot.
# NETWORK
#
# The IP address can be found in the PolyScope interface (tablet) of the robot.
# SETUP Robot -> Setup NETWORK (requires password) -> IP address
UR5_IP = "129.241.187.119"
UR5_HOSTNAME = 'ur-2012208984' #requires dns.
# 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.
# BLOCK DIMENSION
#
# Blocks are cubic and the hole is centered.
BLOCK_DIM = 0.0975 #not exactly 10cm
# TABLE CONFIGURATION
#
# The table configuration specifies parameters for the cylinder coordinate system.
# All table parameters are relative to the robots base coordinate system.
#
# TABLE_QUADRANT was added because the UR5's kinetmatics computations are dependent
# on its own base coordinate system. For instance, quadrant 3.0 results in joint
# violations in the original setup. Quadrant 0.0 corresponds to the table center
# 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_ARC = math.pi/2
# HOME JOINT CONFIGURATION
#
# This is the joint config for the robot's initial position.
# We need this also expressed in joint space to ensure that the robot does not
# collide with itself when moving to the base configuration, and to ensure
# the correct tool orientation when calibrating the reference cylinder
# coordinate system.
#
# Change joints 2-6 in QUADRANT_JOINTS_HOME to tune the tool orientation.
R_HOME = 0.5
THETA_HOME = 0
Z_HOME = TABLE_Z + 2*BLOCK_DIM
JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
-3*math.pi/4,
-3*math.pi/4,
-math.pi/2,
-math.pi/2,
49*math.pi/50]
# FORCE MONITOR
#
# Enables force constraints on the tool (for manual control).
# When the force exceeds FORCE_CONSTRAINT (newton), the robot move in the direction
# opposite to the current command in T_FORCE_VIOLATION seconds.
# 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.
USE_FORCE_MONITOR = False
FORCE_CONSTRAINT = 60
FORCE_T = 0.1
T_FORCE_VIOLATION = 0.1
# Control parameters.
VELOCITY = 0.1
ACCELERATION = 0.5
DECELERATION = 0.5
CHCMD_DECEL = 1.0
CHCMD_T = 0.1 # 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
# MANUAL CONTROL
#
# Parameters that specifies velocity curve and constraints of manual control.
# Note that the angular velocity is also [m/s], not [rad/s], to ensure that the robot
# moves with constant speed regardless of direction in the polar plane.
# IMPORTANT: VEL_Z should be tuned to Z_MIN to avoid smashing into the table.
VEL = 0.1 #[m/s]
VEL_Z = 0.05 #[m/s]
ACC = 1.0 #[m/s^2]
STOP_ACC = 1.0 #aka deceleration
T_DIR_CHANGE_COOLDOWN = 0.1
# 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.5 # relative to TABLE center
R_MAX = 0.8 # 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
# Constraints are relative to the cylinder coordinate system
R_MIN = 0.5
R_MAX = 0.8
THETA_MIN= -TABLE_ARC/2
THETA_MAX = TABLE_ARC/2
Z_MIN = TABLE_Z + 0.6*BLOCK_DIM
Z_MAX = TABLE_Z + 4.5*BLOCK_DIM #4 blocks high
# 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
# AUTO/LOOP CONTROL
#
# When looping, the robot use the same velocity for r, theta and z.
# Also, since this is all planned motions, we dont need to use stopl().
# This means that the trapezoidal velocity curve has the same slope at
# start and end of a motion. (acceleration=deceleration)
#
# Constraints in r and z-direction are specefied in levels to enable
# easier construction of loops. Note that the r-levels grow inwards, and
# the z-levels grow upwards.
DEFAULT_LOOP_VEL = 0.1
LOOP_VEL_MIN = 0.05
LOOP_VEL_MAX = 0.7
LOOP_ACC = 0.5
R_LVL0 = 0.7 #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
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
# Offsets when picking (and placing) blocks.
# The place-block move is the reverse of the pick-block move,
# such that the start/end offsets for pick-block are the end/start offsets
# for place-block
R_STARTPICK_OFFSET = -1.3*BLOCK_DIM
THETA_STARTPICK_OFFSET = 0
Z_STARTPICK_OFFSET = 0
R_ENDPICK_OFFSET = -0.01 #lift slightly inwards to avoid collision with adjacent blocks
THETA_ENDPICK_OFFSET = 0
Z_ENDPICK_OFFSET = BLOCK_DIM*0.5
# 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,
49*math.pi/50]

BIN
config.pyc
View File

Binary file not shown.

308
ur5controller.py
View File

@ -20,27 +20,34 @@ class DemoController(object):
except Exception, e:
raise UR5Exception('Robot initiation failed: %s' % str(e))
# Control params
self.vel = config.VELOCITY
self.acc = config.ACCELERATION
self.dec = config.DECELERATION
self.chcmd_decel = config.CHCMD_DECEL
self.t_chcmd = config.CHCMD_T
self.prev_vec = (0,0,0)
self.t_ch = 0
self.force_mon = config.USE_FORCE_MONITOR
self.force_constraint = config.FORCE_CONSTRAINT
self.ft = config.FORCE_T
#self.controllable = False
# Cylinder params
# Cylinder coordinate system params
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)
# Home pose params
self.j_home = config.JOINTS_HOME
self.r_home = config.R_HOME
self.theta_home = config.THETA_HOME
self.z_home = config.Z_HOME
# Force monitor params
self.force_mon = config.USE_FORCE_MONITOR
self.force_constraint = config.FORCE_CONSTRAINT
self.t_force = config.T_FORCE_VIOLATION
# Manual control velocity params
self.vel = config.VEL
self.vel_z = config.VEL_Z
self.acc = config.ACC
self.stop_acc = config.STOP_ACC
self.t_chcmd = config.T_DIR_CHANGE_COOLDOWN
self.prev_vec = (0,0,0)
self.t_ch = 0
# Manual control constraints
self.r_min = config.R_MIN
self.r_max = config.R_MAX
self.theta_min = config.THETA_MIN
@ -48,41 +55,40 @@ class DemoController(object):
self.z_min = config.Z_MIN
self.z_max = config.Z_MAX
# Block params
# Block move 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
def set_pose_home(self):
self.r_pick_init = config.R_STARTPICK_OFFSET
self.theta_pick_init = config.THETA_STARTPICK_OFFSET
self.z_pick_init = config.Z_STARTPICK_OFFSET
self.r_pick_end = config.R_ENDPICK_OFFSET
self.theta_pick_end = config.THETA_ENDPICK_OFFSET
self.z_pick_end = config.Z_ENDPICK_OFFSET
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)
pose = self.robot.getl()
pose[0] = self.r_min*math.cos(self.phi) + self.cyl_ox
pose[1] = self.r_min*math.sin(self.phi) + self.cyl_oy
pose[2] = self.z_min
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.current_cyl = (self.r_min, 0, self.z_min)
self.current_cyl = (self.r_home, self.theta_home, self.z_home)
if self.debug:
print "initial cyl coords: %s" % str([('%.2f' % i) for i in self.current_cyl])
print "Initial cylinder coords: %s" % str([('%.2f' % i) for i in self.current_cyl])
except Exception, e:
raise UR5Exception('Move to home configuration failed: %s' % str(e))
def calibrate_csys(self):
def calibrate_cylinder_sys(self):
"""Calibrate the reference cylinder coordinate system."""
# Move to reference coordinate system base
self.set_pose_home()
self.move_to_home_pose()
# Set reference coordinate system parameters
csys = math3d.Transform()
csys.orient.rotate_zb(self.phi)
@ -90,29 +96,9 @@ class DemoController(object):
self.trans_base = self.robot.get_transform()
self.cyl_ox = self.cyl_offset
self.cyl_oy = 0
#self.controllable = True
self.set_current_cyl()
if self.debug:
print "Current cyl coords: %s" % str([('%.2f' % i) for i in self.current_cyl])
def set_freedrive(self, mode):
"""Set UR5 to freedrive mode."""
self.robot.set_freedrive(mode)
def cylinder2pose(self, r, theta, z):
"""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,
r*math.sin(theta) + self.cyl_oy,
z)
return trans.pose_vector.tolist()
def set_current_cyl(self):
def set_cylinder_coords(self):
p = self.robot.getl()
x = p[0] - self.cyl_ox
y = p[1] - self.cyl_oy
@ -122,14 +108,36 @@ class DemoController(object):
self.current_cyl = (r, theta, z)
def movel(self, r, theta, z, wait=True):
"""Linear move."""
pose = self.cylinder2pose(r, theta, z)
self.robot.movel(pose, acc=self.acc, vel=self.vel, radius=0, wait=wait, relative=False)
self.current_cyl = (r, theta, z)
def cylinder2cartesian(self, r, theta, z):
"""Translate table cylinder coordinates to reference cartesian coordinates."""
trans = math3d.Transform(self.trans_base) #deep copy
trans.orient.rotate_zb(theta)
trans.pos = math3d.Vector(r*math.cos(theta) + self.cyl_ox,
r*math.sin(theta) + self.cyl_oy,
z)
return trans.pose_vector.tolist()
def movec(self, r, theta, z):
def exec_move(self, move, wait=False):
"""Execute a linear move. move_list = pose + [acc, vel, radius]"""
prog = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})".format(*move)
self.robot.send_program(prog)
if wait:
self.robot.wait_for_move()
def exec_moves(self, move_list, wait=False):
"""Executes a list of linear moves. 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 wait:
self.robot.wait_for_move()
def movec(self, r, theta, z, wait=False):
"""Circular move - uses URScript built-in function.
Unfortunately, this doesn't work very well: The tool orientation at the target pose
@ -137,37 +145,21 @@ class DemoController(object):
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)
pose_via = self.cylinder2cartesian(r, curr_theta+(theta-curr_theta)/2, z)
pose = self.cylinder2cartesian(r, theta, z)
self.robot.movec(pose_via, pose, acc=self.acc, vel=self.vel, radius=0, wait=wait)
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 linearize_arc(self, r, theta, z, acc=None, vel=None, blend_last=0):
def linearize_arc(self, r, theta, z, resolution=math.pi/41, blend=0.03):
"""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
by linearizing the arc into segments with length given in resolution, and use
movels with blending.
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
step = resolution
curr_r, curr_theta, curr_z = self.current_cyl
dtheta = theta - curr_theta
@ -177,85 +169,40 @@ class DemoController(object):
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 + [acc or self.acc, vel or self.vel, 0.03])
move_list.append(self.cylinder2pose(r, theta, z) + [acc or self.acc, vel or self.vel, blend_last])
pose = self.cylinder2cartesian(curr_r + i*r_incr, curr_theta + i*theta_incr, curr_z + i*z_incr)
move_list.append(pose + [self.acc, self.vel, blend])
move_list.append(self.cylinder2cartesian(r, theta, z) + [self.acc, self.vel, 0]) #dont blend last
return move_list
def movec_hax(self, r, theta, z, blend_last=0, wait=True):
def movec_hax(self, r, theta, z, blend_last=0, wait=False):
"""movec with linearized arc."""
# manual acc/vel
move_list = self.linearize_arc(r, theta, z)
if self.debug:
print "move list for movec_hax"
for p in move_list:
print [('%.3f' % i) for i in p]
self.movels(move_list, wait=wait)
self.current_cyl = (r, theta, z)
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 blocklvl2arc(self, r_lvl, theta, z_lvl, acc=None, vel=None, blend_last=0):
r = self.r_lvl0 - r_lvl*self.block_dim
z = self.z_lvl0 + z_lvl*self.block_dim
return self.linearize_arc(r, theta, z, acc=acc, vel=vel, blend_last=blend_last)
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)
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."""
moves = self.pick_block(r_lvl, theta, z_lvl, speed)
moves.reverse()
return moves
def construct_moves(self, task_list, speed):
"""Construct moves from a list of tasks"""
pass # wait with implementing this
def is_looping(self, dt, threshold=0.001):
"""Polls the robot for change in pose vector. Blocks for dt seconds."""
pose0 = self.robot.getl()
time.sleep(dt)
pose1 = self.robot.getl()
v = [pose1[i]-pose0[i] for i in range(len(pose0))]
if self.debug:
_sum = sum(map(abs, v))
print "Velocity vector, dt=%.2f: %s. SUM %.3f" % (dt, str([('%.2f' % i) for i in v]), _sum)
return sum(map(abs, v)) > threshold
self.exec_moves(move_list, wait=wait)
def move(self, vec, t=0):
"""Move in vec direction. Assumes that |v| != 0"""
vr, vtheta, vz = vec
r, theta, z = self.current_cyl
if vtheta == 0:
# linear move
if vz == 0:
move =
else:
# arc move
if vr != 0 or (vr != 0 and vtheta != 0):
if vr != 0:
self.movel(r+vr, theta, z, wait=False)
elif vtheta != 0:
@ -268,7 +215,6 @@ class DemoController(object):
time.sleep(t)
self.stopl(acc=self.chcmd_decel)
def update(self, vec, dt):
"""Update movements based on vec and dt"""
@ -320,5 +266,67 @@ class DemoController(object):
self.prev_vec = vec
self.t_ch = 0
def is_looping(self, dt, threshold=0.001):
"""Polls the robot for change in pose vector. Blocks for dt seconds."""
pose0 = self.robot.getl()
time.sleep(dt)
pose1 = self.robot.getl()
v = [pose1[i]-pose0[i] for i in range(len(pose0))]
if self.debug:
_sum = sum(map(abs, v))
print "Velocity vector, dt=%.2f: %s. SUM %.3f" % (dt, str([('%.2f' % i) for i in v]), _sum)
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
return self.cylinder2pose(self.r_lvl0 - r_lvl*self.block_dim, theta, self.z_lvl0 + z_lvl*self.block_dim)
def pick_block(self, r_lvl, theta, z_lvl, acc, vel):
"""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.
"""
# 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)
move_list = [
p1 + [self.acc, vel, 0.005],
p2 + [self.acc, vel, 0],
p3 + [self.acc, vel, 0.01],
]
return move_list
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.reverse()
return moves
def construct_moves(self, task_list, acc, vel):
"""Construct moves from a list of tasks"""
pass # wait with implementing this
def set_freedrive(self, mode):
self.robot.set_freedrive(mode)
def cleanup(self):
self.robot.cleanup()

BIN
ur5controller.pyc
View File

Binary file not shown.