128 lines
3.9 KiB
Python
128 lines
3.9 KiB
Python
import urx, math, math3d
|
|
|
|
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))
|
|
|
|
# Set cylinder parameters
|
|
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.TABLE_EDGE_LEFT
|
|
self.theta_max = config.TABLE_EDGE_RIGHT
|
|
self.z_min = config.Z_MIN
|
|
self.z_max = config.Z_MAX
|
|
|
|
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()
|
|
|
|
# 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
|
|
|
|
|
|
|
|
def set_pose_home(self):
|
|
"""Move to "home" configuration. The resulting pose ensures 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)
|
|
except Exception, e:
|
|
raise UR5Exception('Move to home configuration failed: %s' % str(e))
|
|
|
|
|
|
def cylinder2pose(self, r, theta, z):
|
|
"""Translate table cylinder coordinates to robot cartesian pose.
|
|
(r, theta) is given in TABLE 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 movel(self, r, theta, z):
|
|
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.
|
|
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 movec_hax(self, r, theta, z):
|
|
"""Since movec is unreliable in reaching the end pose: linearize the arc."""
|
|
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))
|
|
|
|
#debug
|
|
if self.debug:
|
|
print "pose list for movec_hax"
|
|
for p in pose_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.current_cyl = (r, theta, z)
|
|
|
|
|
|
|
|
def cleanup(self):
|
|
self.robot.cleanup()
|