diff --git a/config.py b/config.py index 6f24a9d..dd03964 100644 --- a/config.py +++ b/config.py @@ -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. diff --git a/config.pyc b/config.pyc index ad1357e..51ea7d1 100644 Binary files a/config.pyc and b/config.pyc differ diff --git a/demo.py b/demo.py index edd075d..842c655 100755 --- a/demo.py +++ b/demo.py @@ -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 diff --git a/unittesting.py b/unittesting.py index d7b4b17..04a0fb3 100755 --- a/unittesting.py +++ b/unittesting.py @@ -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: diff --git a/ur5controller.py b/ur5controller.py index 75d9e56..f17fcf9 100644 --- a/ur5controller.py +++ b/ur5controller.py @@ -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() diff --git a/ur5controller.pyc b/ur5controller.pyc index 035e720..8c867d2 100644 Binary files a/ur5controller.pyc and b/ur5controller.pyc differ