diff --git a/config.py b/config.py index 62559e5..6fa8d66 100644 --- a/config.py +++ b/config.py @@ -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 diff --git a/config.pyc b/config.pyc index b195b2a..12e199d 100644 Binary files a/config.pyc and b/config.pyc differ diff --git a/demo.py b/demo.py index 842c655..925a7e5 100755 --- a/demo.py +++ b/demo.py @@ -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.") diff --git a/ur5controller.py b/ur5controller.py index 916e222..7a2d804 100644 --- a/ur5controller.py +++ b/ur5controller.py @@ -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: diff --git a/ur5controller.pyc b/ur5controller.pyc index 656e61d..a4af8ef 100644 Binary files a/ur5controller.pyc and b/ur5controller.pyc differ