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. # opposite to the current command in T_FORCE_VIOLATION seconds.
# Note: Setting this to "True" involves polling the UR5 through a real-time # 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. # monitor that runs at 125Hz. If the controller runs slow, try disabling this.
USE_FORCE_MONITOR = False USE_FORCE_MONITOR = True
FORCE_CONSTRAINT = 60 FORCE_CONSTRAINT = 60
T_FORCE_VIOLATION = 0.1 T_FORCE_VIOLATION = 0.1
@ -92,12 +92,16 @@ ACC = 1.0 #[m/s^2]
STOP_ACC = 1.0 #aka deceleration STOP_ACC = 1.0 #aka deceleration
T_DIR_CHANGE_COOLDOWN = 0.1 T_DIR_CHANGE_COOLDOWN = 0.1
# Constraints are relative to the cylinder coordinate system # Constraints are relative to the cylinder coordinate system.
R_MIN = 0.5 # Because of delay and fluctuations of parameters in the system,
R_MAX = 0.8 # 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_MIN= -TABLE_ARC/2
THETA_MAX = 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 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.clock = pygame.time.Clock()
self.sig_quit = False self.sig_quit = False
self.loop_speed = config.DEFAULT_LOOP_SPEED self.loop_speed = config.DEFAULT_LOOP_VEL
self.controller = None self.controller = None
def _disptxt(self, s_arr): def _disptxt(self, s_arr):
@ -54,7 +54,7 @@ class UR5Demo(object):
def go_home_pos(self): def go_home_pos(self):
self._disptxt(["Going to home configuration..."]) 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): def freedrive(self):
self.controller.set_freedrive(True) self.controller.set_freedrive(True)
@ -81,10 +81,10 @@ class UR5Demo(object):
self.sig_quit = True self.sig_quit = True
if pressed[pygame.K_PLUS] or pressed[pygame.K_KP_PLUS]: if pressed[pygame.K_PLUS] or pressed[pygame.K_KP_PLUS]:
self.loop_speed += incr 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]: elif pressed[pygame.K_MINUS] or pressed[pygame.K_KP_MINUS]:
self.loop_speed -= incr 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: elif pressed[pygame.K_ESCAPE] or self.sig_quit:
running = False running = False
@ -97,7 +97,7 @@ class UR5Demo(object):
def run_loop1(self): def run_loop1(self):
loopcount = 0 loopcount = 0
self._disptxt(["Running loop 1", "Loopcount: %d" % loopcount]) self._disptxt(["Running loop \"four blocks high\"", "Loopcount: %d" % loopcount])
time.sleep(1) time.sleep(1)
# initiate loop, poll for robot move change and keypress # initiate loop, poll for robot move change and keypress
@ -116,20 +116,29 @@ class UR5Demo(object):
def run_loop2(self): def run_loop2(self):
loopcount = 0 loopcount = 0
self._disptxt(["Running loop 2", "Loopcount: %d" % loopcount]) self._disptxt(["Running loop \"small pyramid\"", "Loopcount: %d" % loopcount])
time.sleep(1) 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): def loop_init(self, block_init_msg, loopnum):
msg = [ msg = [
"Initital block configuration for loop %d:" % loopnum,
block_init_msg,
" ",
"Current loop speed is %.2f m/s" % self.loop_speed, "Current loop speed is %.2f m/s" % self.loop_speed,
"Start loop? [ENTER]" "Start loop? [ENTER]"
] ]
self._disptxt(msg) self._disptxt(block_init_msg + msg)
running = True running = True
while running: while running:
pressed = pygame.key.get_pressed() pressed = pygame.key.get_pressed()
@ -150,6 +159,7 @@ class UR5Demo(object):
self.clock.tick(config.MENU_FPS) self.clock.tick(config.MENU_FPS)
def keyboard_control(self): def keyboard_control(self):
t = time.time()
running = True running = True
while running: while running:
pressed = pygame.key.get_pressed() 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]): for m in (self.kb_map[key] for key in self.kb_map if pressed[key]):
vec = map(sum, zip(vec, m)) vec = map(sum, zip(vec, m))
self._disptxt([ new_t = time.time()
"Keyboard control", dt = max(new_t-t, 1/config.CTR_FPS)
"Use arrow keys, [w], and [s]", t = new_t
" ",
"move vec: %s" % str(vec)
])
dt = 1.0/config.CTR_FPS
try: try:
self.controller.update(vec, dt) self.controller.update(tuple(vec), dt)
except Exception, e: except Exception, e:
self._disptxt(["Error: %s" % e]) self._disptxt(["Error: %s" % e])
time.sleep(2) time.sleep(2)
running = False 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) self.clock.tick(config.CTR_FPS)
@ -186,8 +201,8 @@ class UR5Demo(object):
"Go to [h]ome position", "Go to [h]ome position",
"[f]reedrive mode", "[f]reedrive mode",
"Adjust loop [s]peed", "Adjust loop [s]peed",
"Start loop [1]", "Start loop [1]: four blocks high",
"Start loop [2]", "Start loop [2]: small pyramid",
"Manual [c]ontrol", "Manual [c]ontrol",
"[q]uit" "[q]uit"
] ]
@ -212,12 +227,25 @@ class UR5Demo(object):
self._disptxt(menu) self._disptxt(menu)
elif pressed[pygame.K_1]: 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.loop_init(_instr, 1)
self._disptxt(menu) self._disptxt(menu)
elif pressed[pygame.K_2]: 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.loop_init(_instr, 2)
self._disptxt(menu) self._disptxt(menu)
@ -240,9 +268,9 @@ class UR5Demo(object):
try: try:
self.controller = DemoController(config) self.controller = DemoController(config)
self.controller.set_freedrive(False) self.controller.set_freedrive(False)
msg.append("Calibrating cylinder coordinates...") msg.append("Calibrating cylinder coordinate system...")
self._disptxt(msg) self._disptxt(msg)
self.controller.calibrate_csys() self.controller.calibrate_cylinder_sys()
except Exception, e: except Exception, e:
return self.quit(error=("Error: %s" % e)) return self.quit(error=("Error: %s" % e))
msg.append("Calibration done.") msg.append("Calibration done.")

60
ur5controller.py
View File

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

BIN
ur5controller.pyc
View File

Binary file not shown.