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

controller is almost up and running. Need some fixes and improvements though.

Browse Source
This commit is contained in:
Michael Soukup 2014-08-04 22:11:23 +02:00
parent 50b7f1a1ef
commit 496bc6bba2
6 changed files with 261 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
config.py
View File

@ -8,7 +8,7 @@ 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
# IP of the robot.
UR5_IP = "129.241.187.119"
@ -17,17 +17,23 @@ UR5_IP = "129.241.187.119"
# 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.0995 #TODO measure
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.

BIN
config.pyc
View File

Binary file not shown.

36
demo.py
View File

@ -149,6 +149,38 @@ class UR5Demo(object):
self.clock.tick(config.MENU_FPS)
def keyboard_control(self):
running = True
while running:
pressed = pygame.key.get_pressed()
for e in pygame.event.get():
if e.type == pygame.QUIT:
self.sig_quit = True
if pressed[pygame.K_ESCAPE] or self.sig_quit:
running = False
vec = (0,0,0)
for m in (self.kb_map[key] for key in self.kb_map if pressed[key]):
vec = map(sum, zip(vec, m))
self._disptxt([
"Keyboard control",
"Use arrow keys, [w], and [s]",
" ",
"move vec: %s" % str(vec)
])
dt = 1.0/config.CTR_FPS
try:
self.controller.update(vec, dt)
except Exception, e:
self._disptxt(["Error: %s" % e])
time.sleep(2)
running = False
self.clock.tick(config.CTR_FPS)
def main_menu(self):
menu = [
"Go to [h]ome position",
@ -189,6 +221,10 @@ class UR5Demo(object):
self.loop_init(_instr, 2)
self._disptxt(menu)
elif pressed[pygame.K_c]:
self.keyboard_control()
self._disptxt(menu)
elif pressed[pygame.K_q] or self.sig_quit:
running = False

113
unittesting.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys, config, math, time
import sys, config, math, time, traceback
from ur5controller import DemoController
@ -103,35 +103,100 @@ def test_pick_place(controller):
def small_pyramid(controller):
# Initial block placement.
# On each side
# (r0, edge, z0)
# (r1, edge, z0)
# (r0, edge, z1)
dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap
speed = 0.2
acc = config.ACCELERATION
moves = []
# Pick left (r0, edge, z1), place in middle
moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 1, speed)
tmp_pose = controller.blocklvl2pose(0.2, -dtheta_lvl0, 1.1)
moves.append(tmp_pose + [acc, speed, 0.05])
moves += controller.place_block(0, 0, 0, speed)
# Pick left (r1, edge, z0), place left
moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed)
tmp_pose = controller.blocklvl2pose(1.1, config.THETA_EDGE_LEFT+dtheta_lvl0, 0.5)
moves.append(tmp_pose + [acc, speed, 0.05])
moves += controller.place_block(0, -dtheta_lvl0, 0, speed)
# Pick right (r0, edge, z1), place right
tmp_pose = controller.blocklvl2pose(1.3, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.2)
moves.append(tmp_pose + [acc, speed, 0.05])
#moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_RIGHT - math.pi/16, 1, vel=speed, blend_last=0.03)
moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 1, speed)
tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.2)
moves.append(tmp_pose + [acc, speed, 0.03])
moves += controller.place_block(0, dtheta_lvl0, 0, speed)
# Pick right (r1, edge, z0), place lvl1 right
moves += controller.pick_block(1, config.THETA_EDGE_RIGHT, 0, speed)
tmp_pose = controller.blocklvl2pose(1.1, config.THETA_EDGE_RIGHT-dtheta_lvl0, 1.1)
moves.append(tmp_pose + [acc, speed, 0.03])
moves += controller.place_block(0, 0.5*dtheta_lvl0, 1, speed)
# Pick left (r0, edge, z0), place lvl1 left
moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_LEFT + math.pi/16, 0.1, vel=speed, blend_last=0.03)
moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 0, speed)
tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_LEFT+dtheta_lvl0, 1.1)
moves.append(tmp_pose + [acc, speed, 0.03])
moves += controller.place_block(0, -0.5*dtheta_lvl0, 1, speed)
# Pick right (r0, edge, z0), place on top
moves += controller.blocklvl2arc(1.3, config.THETA_EDGE_RIGHT - math.pi/16, 0, vel=speed, blend_last=0.03)
moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 0, speed)
tmp_pose = controller.blocklvl2pose(0, config.THETA_EDGE_RIGHT-dtheta_lvl0, 2.1)
moves.append(tmp_pose + [acc, speed, 0.03])
moves += controller.place_block(0, 0, 2, speed)
# Execute moves
controller.movels(moves, wait=False)
looping = True
while looping:
looping = controller.is_looping(1.0) # blocks for 1 sec
moves.reverse()
controller.movels(moves, wait=True)
looping = True
while looping:
looping = controller.is_looping(1.0) # blocks for 1 sec
def big_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 block placement.
# On each side
# (r0, edge, z0)
# (r1, edge, z0)
# (r0, edge, z1)
# (r1, edge, z1)
# (r0, edge, z2)
# (r1, edge, z2)
speed = 0.2
# Initial: 2x2 on each side
moves = []
#move += controller.blocklvl2arc(0, config.THETA_EDGE_LEFT + math.pi/16, 1, vel=speed, blend_last=0.03)
moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 1, speed) # Pick left (r0, edge, z1)
moves += controller.place_block(0, -0.5*dtheta_lvl0, 0)
moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed) # Pick left (r1, edge, z0)
moves += controller.place_block(0, -1.5*dtheta_lvl0, 0)
controller.blocklvl2pose(r_lvl, theta, z_lvl) + [acc, vel, rad]
moves += controller.blocklvl2arc(0.1, config.THETA_EDGE_RIGHT - math.pi/16, 1, vel=speed, blend_last=0.03)
moves += controller.pick_block(0, config.THETA_EDGE_RIGHT, 1, speed) # Pick right (r0, edge, z1)
moves += controller.place_block(0, 0.5*dtheta_lvl0, 0)
moves += controller.pick_block(1, config.THETA_EDGE_RIGHT, 0, speed) # Pick right (r1, edge, z0)
moves += controller.place_block(0, 1.5*dtheta_lvl0, 0)
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__':
@ -143,12 +208,14 @@ if __name__ == '__main__':
#check_edges(controller)
#test_movec_hax(controller)
#check_theta_edges(controller)
test_pick_place(controller)
#test_pick_place(controller)
#pick_place_small_pyramid(controller)
small_pyramid(controller)
# NEXT: test radial params and arbitrary paths.
except Exception, e:
print e
print traceback.format_exc()
time.sleep(1)
if controller:

153
ur5controller.py
View File

@ -24,6 +24,15 @@ class DemoController(object):
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
@ -54,6 +63,22 @@ class DemoController(object):
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
@ -65,6 +90,12 @@ 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):
@ -72,20 +103,6 @@ class DemoController(object):
self.robot.set_freedrive(mode)
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)
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."""
trans = math3d.Transform(self.trans_base) #deep copy
@ -95,15 +112,20 @@ 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 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):
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=True, relative=False)
self.robot.movel(pose, acc=self.acc, vel=self.vel, radius=0, wait=wait, relative=False)
self.current_cyl = (r, theta, z)
@ -136,11 +158,10 @@ class DemoController(object):
self.robot.wait_for_move()
return self.robot.getl()
def movec_hax(self, r, theta, z):
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
movepls with blend.
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
@ -154,20 +175,34 @@ class DemoController(object):
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])
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=True)
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:
@ -216,5 +251,71 @@ class DemoController(object):
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:
self.movec_hax(r, theta+vtheta, 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()
vr, vtheta, vz = vec
r, theta, z = self.current_cyl
# move?
if sum(map(abs, vec)) == 0:
if vec != self.prev_vec:
self.robot.stopl(acc=self.chcmd_decel)
prev_vec = (0,0,0)
return
# check bounds
rnext = r + (vr*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
if thetanext < self.theta_min or thetanext > self.theta_max:
vtheta = 0
znext = z + (vz*self.vel*dt)
if znext < self.z_min or znext > self.z_max:
vz = 0
vec = (vr, vtheta, vz)
# 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()

BIN
ur5controller.pyc
View File

Binary file not shown.