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

CONSTRAINTS FIXED. FUCKYEAH

Browse Source
This commit is contained in:
Michael Soukup 2014-08-06 00:42:00 +02:00
parent 5ee0353e01
commit f4ca1805f8
5 changed files with 106 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
config.py
View File

@ -74,7 +74,7 @@ JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
# 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
USE_FORCE_MONITOR = True
FORCE_CONSTRAINT = 60
T_FORCE_VIOLATION = 0.1
@ -92,12 +92,16 @@ ACC = 1.0 #[m/s^2]
STOP_ACC = 1.0 #aka deceleration
T_DIR_CHANGE_COOLDOWN = 0.1
# Constraints are relative to the cylinder coordinate system
R_MIN = 0.5
R_MAX = 0.8
# Constraints are relative to the cylinder coordinate system.
# Because of delay and fluctuations of parameters in the system,
# the calculation of projected positions are not correct.
# Therefore, an empirical offset is added in controller.update().
# Consider this before changing control speed
R_MIN = 0.49
R_MAX = 0.75
THETA_MIN= -TABLE_ARC/2
THETA_MAX = TABLE_ARC/2
Z_MIN = TABLE_Z + 0.6*BLOCK_DIM
Z_MIN = TABLE_Z + 0.5*BLOCK_DIM
Z_MAX = TABLE_Z + 4.5*BLOCK_DIM #4 blocks high

BIN
config.pyc
View File

Binary file not shown.

78
demo.py
View File

@ -26,7 +26,7 @@ class UR5Demo(object):
self.clock = pygame.time.Clock()
self.sig_quit = False
self.loop_speed = config.DEFAULT_LOOP_SPEED
self.loop_speed = config.DEFAULT_LOOP_VEL
self.controller = None
def _disptxt(self, s_arr):
@ -54,7 +54,7 @@ class UR5Demo(object):
def go_home_pos(self):
self._disptxt(["Going to home configuration..."])
self.controller.movel(config.R_MIN, 0, config.Z_MIN)
self.controller.movel(config.R_HOME, config.THETA_HOME, config.Z_HOME)
def freedrive(self):
self.controller.set_freedrive(True)
@ -81,10 +81,10 @@ class UR5Demo(object):
self.sig_quit = True
if pressed[pygame.K_PLUS] or pressed[pygame.K_KP_PLUS]:
self.loop_speed += incr
self.loop_speed = min(self.loop_speed, config.LOOP_SPEED_MAX)
self.loop_speed = min(self.loop_speed, config.LOOP_VEL_MAX)
elif pressed[pygame.K_MINUS] or pressed[pygame.K_KP_MINUS]:
self.loop_speed -= incr
self.loop_speed = max(self.loop_speed, config.LOOP_SPEED_MIN)
self.loop_speed = max(self.loop_speed, config.LOOP_VEL_MIN)
elif pressed[pygame.K_ESCAPE] or self.sig_quit:
running = False
@ -97,7 +97,7 @@ class UR5Demo(object):
def run_loop1(self):
loopcount = 0
self._disptxt(["Running loop 1", "Loopcount: %d" % loopcount])
self._disptxt(["Running loop \"four blocks high\"", "Loopcount: %d" % loopcount])
time.sleep(1)
# initiate loop, poll for robot move change and keypress
@ -116,20 +116,29 @@ class UR5Demo(object):
def run_loop2(self):
loopcount = 0
self._disptxt(["Running loop 2", "Loopcount: %d" % loopcount])
self._disptxt(["Running loop \"small pyramid\"", "Loopcount: %d" % loopcount])
time.sleep(1)
pass
# initiate loop, poll for robot move change and keypress
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
self.clock.tick(config.MENU_FPS)
# Set to default pos before return?
def loop_init(self, block_init_msg, loopnum):
msg = [
"Initital block configuration for loop %d:" % loopnum,
block_init_msg,
" ",
"Current loop speed is %.2f m/s" % self.loop_speed,
"Start loop? [ENTER]"
]
self._disptxt(msg)
self._disptxt(block_init_msg + msg)
running = True
while running:
pressed = pygame.key.get_pressed()
@ -150,6 +159,7 @@ class UR5Demo(object):
self.clock.tick(config.MENU_FPS)
def keyboard_control(self):
t = time.time()
running = True
while running:
pressed = pygame.key.get_pressed()
@ -163,21 +173,26 @@ class UR5Demo(object):
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)
])
new_t = time.time()
dt = max(new_t-t, 1/config.CTR_FPS)
t = new_t
dt = 1.0/config.CTR_FPS
try:
self.controller.update(vec, dt)
self.controller.update(tuple(vec), dt)
except Exception, e:
self._disptxt(["Error: %s" % e])
time.sleep(2)
running = False
f = self.controller.zforce
self._disptxt([
"Keyboard control",
"Use arrow keys, [w], and [s]",
" ",
"move vec: %s" % str(vec),
"dt: %.3f" % dt,
"force z: %s %.1f" % (" " if f>0 else "", f)
])
self.clock.tick(config.CTR_FPS)
@ -186,8 +201,8 @@ class UR5Demo(object):
"Go to [h]ome position",
"[f]reedrive mode",
"Adjust loop [s]peed",
"Start loop [1]",
"Start loop [2]",
"Start loop [1]: four blocks high",
"Start loop [2]: small pyramid",
"Manual [c]ontrol",
"[q]uit"
]
@ -212,12 +227,25 @@ class UR5Demo(object):
self._disptxt(menu)
elif pressed[pygame.K_1]:
_instr = "2x2 blocks on left side"
_instr = [
"Initital block configuration for loop \"four blocks high\":",
"On each side",
" (r0, edge, z0)",
" (r1, edge, z0)",
" "
]
self.loop_init(_instr, 1)
self._disptxt(menu)
elif pressed[pygame.K_2]:
_instr = "2x2 blocks on left side, 2x2 blocks on right side"
_instr = [
"Initital block configuration for loop \"small pyramid\":",
"On each side",
" (r0, edge, z0)",
" (r1, edge, z0)",
" (r0, edge, z1)",
" "
]
self.loop_init(_instr, 2)
self._disptxt(menu)
@ -240,9 +268,9 @@ class UR5Demo(object):
try:
self.controller = DemoController(config)
self.controller.set_freedrive(False)
msg.append("Calibrating cylinder coordinates...")
msg.append("Calibrating cylinder coordinate system...")
self._disptxt(msg)
self.controller.calibrate_csys()
self.controller.calibrate_cylinder_sys()
except Exception, e:
return self.quit(error=("Error: %s" % e))
msg.append("Calibration done.")

62
ur5controller.py
View File

@ -45,6 +45,7 @@ class DemoController(object):
self.stop_acc = config.STOP_ACC
self.t_chcmd = config.T_DIR_CHANGE_COOLDOWN
self.prev_vec = (0,0,0)
self.zforce = 0.0
self.t_ch = 0
# Manual control constraints
@ -186,10 +187,14 @@ class DemoController(object):
def move(self, vec, t=0):
"""Move in vec direction. Assumes that |v| != 0"""
"""Move in vec direction."""
dr, dtheta, dz = vec
r, theta, z = self.current_cyl
#if dr == 0 and dtheta == 0 and dz == 0:
# self.robot.stopl(acc=self.stop_acc)
# return
if dtheta == 0: #linear move
if dz == 0:
move = self.cylinder2cartesian(r+dr, theta, z) + [self.acc, self.vel, 0]
@ -214,36 +219,57 @@ class DemoController(object):
def update(self, vec, dt):
"""Update movements based on vec (and dt?)"""
"""Update movements based on vec and dt"""
force = 10 # dummy
if self.force_mon and force > self.force_constraint:
self.move([-1*i for i in vec], t=self.t_force)
self.set_current_cyl()
return
if self.force_mon:
self.zforce = self.robot.get_tcp_force(wait=False)[2]
if abs(self.zforce) > self.force_constraint:
# move opposite to z force
dz = self.zforce/abs(self.zforce)
self.move((0, 0, dz), t=self.t_force)
self.set_cylinder_coords()
return
self.set_current_cyl()
# move?
self.set_cylinder_coords()
dr, dtheta, dz = 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)
self.robot.stopl(acc=self.stop_acc)
self.prev_vec = (0,0,0)
return
# check projected constraints
rnext = r + (dr*self.vel*dt)
if rnext < self.r_min or rnext > self.r_max:
# Projected position does not work. Fuck this, add empirical offsets instead
#curr_d = (r**2 + (r*theta)**2 + z**2)**0.5
#curr_v = abs(curr_d - self.prev_d) / dt
#self.prev_v = curr_v
#self.prev_d = curr_d
# check projected constraints.
rnext = r + dr*0.031
thetanext = theta + dtheta*0.029
znext = z + dz*0.012
if (rnext < self.r_min and dr < 0) or (rnext > self.r_max and dr > 0):
r = self.r_min if dr < 0 else self.r_max
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:
if (thetanext < self.theta_min and dtheta < 0) or (thetanext > self.theta_max and dtheta > 0):
theta = self.theta_min if dtheta < 0 else self.theta_max
dtheta = 0
znext = z + (dz*self.vel_z*dt)
if znext < self.z_min or znext > self.z_max:
if (znext < self.z_min and dz < 0) or (znext > self.z_max and dz > 0):
z = self.z_min if dz < 0 else self.z_max
dz = 0
vec = (dr, dtheta, dz)
self.current_cyl = (r, theta, z)
# stop on |vec| = 0
if sum(map(abs, vec)) == 0:
if vec != self.prev_vec:
self.robot.stopl(acc=self.stop_acc)
self.prev_vec = (0,0,0)
return
# change move
if vec != self.prev_vec:

BIN
ur5controller.pyc
View File

Binary file not shown.