325 lines
10 KiB
Python
325 lines
10 KiB
Python
import urx, math, math3d, time
|
|
|
|
|
|
class UR5Exception(Exception):
|
|
pass
|
|
|
|
|
|
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
|
|
"""
|
|
|
|
def __init__(self, config):
|
|
"""Initializes the controller with parameters specified in config"""
|
|
self.debug = config.DEBUG
|
|
try:
|
|
self.robot = urx.Robot(config.UR5_IP, useRTInterface=config.USE_FORCE_MONITOR) #TODO or hostname
|
|
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
|
|
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.r_min = config.R_MIN
|
|
self.r_max = config.R_MAX
|
|
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
|
|
|
|
|
|
def set_pose_home(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
|
|
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)
|
|
if self.debug:
|
|
print "initial cyl 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):
|
|
"""Calibrate the reference cylinder coordinate system."""
|
|
# Move to reference coordinate system base
|
|
self.set_pose_home()
|
|
# Set reference coordinate system parameters
|
|
csys = math3d.Transform()
|
|
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.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):
|
|
p = self.robot.getl()
|
|
x = p[0] - self.cyl_ox
|
|
y = p[1] - self.cyl_oy
|
|
z = p[2]
|
|
r = (x**2 + y**2)**0.5
|
|
theta = math.atan2(y, x)
|
|
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 movec(self, r, theta, z):
|
|
"""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 linearize_arc(self, r, theta, z, acc=None, vel=None, blend_last=0):
|
|
"""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
|
|
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
|
|
curr_r, curr_theta, curr_z = self.current_cyl
|
|
dtheta = theta - curr_theta
|
|
|
|
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 + [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])
|
|
return move_list
|
|
|
|
def movec_hax(self, r, theta, z, blend_last=0, wait=True):
|
|
"""movec with linearized arc."""
|
|
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
|
|
|
|
|
|
def move(self, vec, t=0):
|
|
vr, vtheta, vz = vec
|
|
r, theta, z = self.current_cyl
|
|
|
|
if vr != 0:
|
|
self.movel(r+vr, theta, z, wait=False)
|
|
elif vtheta != 0:
|
|
# set end point edge
|
|
self.movec_hax(r, vtheta*self.theta_max, z, wait=False)
|
|
elif vz != 0:
|
|
self.movel(r, theta, z+vz, wait=False)
|
|
|
|
if t:
|
|
time.sleep(t)
|
|
self.stopl(acc=self.chcmd_decel)
|
|
|
|
|
|
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.set_current_cyl()
|
|
return
|
|
|
|
self.set_current_cyl()
|
|
r, theta, z = self.current_cyl
|
|
vr, vtheta, vz = vec
|
|
|
|
# check bounds
|
|
rnext = r + (vr*self.vel*dt)
|
|
if (rnext < self.r_min and vr < 0) or (rnext > self.r_max and vr > 0):
|
|
vr = 0
|
|
thetanext = theta + (vtheta*self.vel*dt) # this is angular speed, but the diff is not significant
|
|
if (thetanext < self.theta_min and vtheta < 0) or (thetanext > self.theta_max and vtheta > 0):
|
|
vtheta = 0
|
|
znext = z + (vz*self.vel*dt)
|
|
if (znext < self.z_min and vz < 0) or (znext > self.z_max and vz > 0):
|
|
vz = 0
|
|
vec = (vr, vtheta, vz)
|
|
|
|
# move?
|
|
if sum(map(abs, vec)) == 0:
|
|
if vec != self.prev_vec:
|
|
# stop
|
|
self.robot.stopl(acc=self.chcmd_decel)
|
|
self.prev_vec = (0,0,0)
|
|
return
|
|
|
|
# command change
|
|
if vec != self.prev_vec:
|
|
# from stand still
|
|
|
|
if sum(map(abs, self.prev_vec)) == 0:
|
|
self.move(vec)
|
|
self.prev_vec = vec
|
|
self.t_ch = 0
|
|
|
|
# from another command
|
|
elif not self.t_ch:
|
|
self.t_ch = time.time()
|
|
self.robot.stopl(acc=self.chcmd_decel)
|
|
elif time.time() - self.t_ch > self.t_chcmd:
|
|
self.move(vec)
|
|
self.prev_vec = vec
|
|
self.t_ch = 0
|
|
|
|
def cleanup(self):
|
|
self.robot.cleanup()
|