unit tests and TUNING IN CONFIG

config.py

@@ -27,8 +27,7 @@ UR5_HOSTNAME = 'ur-2012208984' #requires dns.
 # BLOCK DIMENSION
 #
 # Blocks are cubic and the hole is centered.
-BLOCK_DIM = 0.0975 #not exactly 10cm
+BLOCK_DIM = 0.0965 #TUNED
-
 
 
 # TABLE CONFIGURATION
@@ -42,8 +41,8 @@ BLOCK_DIM = 0.0975 #not exactly 10cm
 # along the base x-axis, quadrant 1.0 corresponds to the base y-axis, etc.
 # Quardant 0 is recommended.
 TABLE_QUADRANT = 0 #[0..4)
-TABLE_ORIGO_OFFSET = -0.25 #offset from origo along base x-axis
+TABLE_ORIGO_OFFSET = (-0.245, -0.003) #offset from origo along base x-axis
-TABLE_Z = -0.336 #this should be fine-tuned on each setup
+TABLE_Z = -0.3337 #this should be fine-tuned on each setup
 TABLE_ARC = math.pi/2
 
 
@@ -57,9 +56,9 @@ TABLE_ARC = math.pi/2
 # coordinate system.
 #
 # Change joints 2-6 in QUADRANT_JOINTS_HOME to tune the tool orientation.
-R_HOME = 0.5
+R_HOME = 0.55
 THETA_HOME = 0
-Z_HOME = TABLE_Z + 2*BLOCK_DIM
+Z_HOME = TABLE_Z + BLOCK_DIM
 JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
 			   	   		-3*math.pi/4,
 			   	   		-3*math.pi/4,
@@ -67,8 +66,6 @@ JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
 			   	  		-math.pi/2,
 			  	  		49*math.pi/50]
 
-<<<<<<< HEAD
-
 
 # FORCE MONITOR
 #
@@ -120,49 +117,10 @@ LOOP_VEL_MIN = 0.05
 LOOP_VEL_MAX = 0.7
 LOOP_ACC = 0.5
 
-R_LVL0 = 0.7 #tip of tool at table edge
+R_LVL0 = 0.691 #tip of tool at table edge
 Z_LVL0 = TABLE_Z + BLOCK_DIM/2 #tool in middle of block
-THETA_EDGE_LEFT = -math.pi/4 + math.pi/68 #empirical. table is not exact
+THETA_EDGE_LEFT = -math.pi/4 + math.pi/74 #empirical. table is not exact
-=======
+THETA_EDGE_RIGHT = math.pi/4 - math.pi/74
-# Enables force constraints on the tool. 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 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.0975 #TODO measure
-
-# Table cylinder parameters
-TABLE_QUADRANT = 0 # In range [0..4). Quadrant 0 is along the x-axis of the robot.
-TABLE_ORIGO_OFFSET = -0.25 # TODO measure
-Z_TABLE = -0.336
-R_MIN = 0.6 # relative to TABLE center
-R_MAX = 0.9 # relative to TABLE center
-THETA_MIN= -math.pi/4 # table edge left
-THETA_MAX = math.pi/4 # table edge right
-Z_MIN = Z_TABLE + BLOCK_DIM/2
-Z_MAX = Z_MIN + 5*BLOCK_DIM
-
-# Block move parameters
-R_LVL0 = 0.7 # tip of tool at table edge
-Z_LVL0 = Z_MIN
-THETA_EDGE_LEFT = -math.pi/4 + math.pi/68
->>>>>>> parent of ccb5ffb... manual controller done. Going to separate control parameters for manual mode and auto mode
-THETA_EDGE_RIGHT = math.pi/4 - math.pi/68
 
 # Offsets when picking (and placing) blocks.
 # The place-block move is the reverse of the pick-block move,

unittesting.py

@@ -5,66 +5,179 @@ import sys, config, math, time, traceback
 from ur5controller import DemoController
 
 
-def check_edges(controller):
+def check_z_levels(controller):
-    controller.movel(config.R_MIN, config.THETA_MAX, config.Z_MIN)
+    print 'Going to home pose.'
-    print 'Right edge.'     
+    controller.move_to_home_pose()
+    time.sleep(1)
+
+    r, theta, z = controller.current_cyl
+    
+    print "z-level 0"
+    z = config.Z_LVL0
+    move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
+    controller.exec_move(move, wait=True)
+    controller.set_cylinder_coords()
     dummy = raw_input('Press any key to continue...')
 
-    controller.movel(config.R_MIN, 0, config.Z_MIN)
+    print "z-level 1"
-    print 'Middle.'
+    z += config.BLOCK_DIM
-    time.sleep(1)
+    move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
+    controller.exec_move(move, wait=True)
+    controller.set_cylinder_coords()
     dummy = raw_input('Press any key to continue...')
 
-    controller.movel(config.R_MIN, config.THETA_MIN, config.Z_MIN)
+    print "z-level 2"
-    print 'Left edge.'
+    z += config.BLOCK_DIM
-    time.sleep(1)
+    move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
+    controller.exec_move(move, wait=True)
+    controller.set_cylinder_coords()
     dummy = raw_input('Press any key to continue...')
 
+    print "z-level 3"
+    z += config.BLOCK_DIM
+    move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
+    controller.exec_move(move, wait=True)
+    controller.set_cylinder_coords()
+    dummy = raw_input('Press any key to continue...')
+
+    print "z-level 4"
+    z += config.BLOCK_DIM
+    move = controller.cylinder2cartesian(r, theta, z) + [0.2, 0.1, 0]
+    controller.exec_move(move, wait=True)
+    controller.set_cylinder_coords()
+    dummy = raw_input('Press any key to continue...')
+
+
+def test_unitary_moves(controller):
+    print 'Going to home pose.'
+    controller.move_to_home_pose()
+    time.sleep(1)
+
+    print 'Moving (+r, 0, 0)'
+    controller.move((1,0,0), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (-r, 0, 0)'
+    controller.move((-1,0,0), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (0, +theta, 0)'
+    controller.move((0,1,0), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (0, -theta, 0)'
+    controller.move((0,-1,0), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (0, 0, +z)'
+    controller.move((0,0,1), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (0, 0, -z)'
+    controller.move((0,0,-1), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+
+def test_multi_moves(controller):
+    print 'Going to home pose.'
+    controller.move_to_home_pose()
+    time.sleep(1)
+
+    print 'Moving (+r, +theta, 0)'
+    controller.move((1,1,0), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (-r, -theta, 0)'
+    controller.move((-1,-1,0), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (+r, -theta, 0)'
+    controller.move((1,-1,0), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (+r, 0, z)'
+    controller.move((1,0,1), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (-r, 0, z)'
+    controller.move((-1,0,1), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (0, theta, -z)'
+    controller.move((0,1,-1), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (0, -theta, -z)'
+    controller.move((0,-1,-1), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (r, theta, z)'
+    controller.move((1,1,1), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+    print 'Moving (-r, theta, -z)'
+    controller.move((-1,1,-1), t=2)
+    time.sleep(1)
+    controller.set_cylinder_coords()
+
+
+def check_loop_constraints(controller):
+    print 'Going to home pose.'
+    controller.move_to_home_pose()
+    time.sleep(1)
+
+    controller.movec_hax(config.R_LVL0, config.THETA_EDGE_RIGHT, config.Z_LVL0, wait=True)
+    controller.set_cylinder_coords()
+    print 'Right edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])     
+    dummy = raw_input('Press any key to continue...')
+
+    controller.movec_hax(config.R_LVL0, 0, config.Z_LVL0, wait=True)
+    controller.set_cylinder_coords()
+    print 'Middle, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])     
+    dummy = raw_input('Press any key to continue...')
+
+    controller.movec_hax(config.R_LVL0, config.THETA_EDGE_LEFT, config.Z_LVL0, wait=True)
+    controller.set_cylinder_coords()
+    print 'Left edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])     
+    dummy = raw_input('Press any key to continue...')
+
+
 def test_movec(controller):
-    controller.movec(config.R_MIN, config.THETA_MAX, config.Z_MIN)
+    print 'Going to home pose.'
-    print 'Right edge.'     
+    controller.move_to_home_pose()
-    dummy = raw_input('Press any key to continue...')
-
-    controller.movec(config.R_MIN, 0, config.Z_MIN)
-    print 'Middle.'
     time.sleep(1)
+
+    controller.movec(config.R_LVL0, config.THETA_EDGE_RIGHT, config.Z_LVL0, wait=True)
+    controller.set_cylinder_coords()
+    print 'Right edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])     
     dummy = raw_input('Press any key to continue...')
 
-    controller.movec(config.R_MIN, config.THETA_MIN, config.Z_MIN)
+    controller.movec(config.R_LVL0, 0, config.Z_LVL0, wait=True)
-    print 'Left edge.'
+    controller.set_cylinder_coords()
-    time.sleep(1)
+    print 'Middle, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])     
     dummy = raw_input('Press any key to continue...')
 
-def test_movec_hax(controller):
+    controller.movec(config.R_LVL0, config.THETA_EDGE_LEFT, config.Z_LVL0, wait=True)
-    controller.movec_hax(config.R_MIN, config.THETA_MAX, config.Z_MIN)
+    controller.set_cylinder_coords()
-    print 'Right edge.'     
+    print 'Left edge, r-level 0, z-level 0. Cylinder coordinates: (%s)' % ','.join(['%.2f' % i for i in controller.current_cyl])     
     dummy = raw_input('Press any key to continue...')
 
-    controller.movec_hax(config.R_MIN, 0, config.Z_MIN)
-    print 'Middle.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
 
-    controller.movec_hax(config.R_MIN, config.THETA_MIN, config.Z_MIN)
-    print 'Left edge.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
 
-def test_movec_hax2(controller):
-    controller.movec_hax(config.R_MIN, config.THETA_MAX, config.Z_MIN)
-    print 'Right edge.'     
-    dummy = raw_input('Press any key to continue...')
-
-    controller.movec_hax(config.R_MIN, 0, config.Z_MIN+0.3)
-    print 'Middle.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
-
-    # NOTE: current R_MIN+0.1=0.7 is where the tooltip is at the edge
-    controller.movec_hax(config.R_MIN+0.1, config.THETA_MIN, config.Z_MIN+0.1)
-    print 'Left edge.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
 
 def check_theta_edges(controller):
     controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN)
@@ -77,6 +190,7 @@ def check_theta_edges(controller):
     print 'Theta edge left.'     
     dummy = raw_input('Press any key to continue...')
 
+
 def test_pick_place(controller):
     dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap
     # #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 #small gap
@@ -198,26 +312,44 @@ def big_pyramid(controller):
     moves += controller.place_block(0, 1.5*dtheta_lvl0, 0)
 
 
+def unit_tests(func_list, controller):
+    running = True
+    while running:
+        for i,tup in enumerate(func_list):
+            print '(%d) %s' % (i, tup[0])
+        c = raw_input('Select test or type q to quit... ')
+        if c == 'q':
+            running = False
+        else:
+            try:
+                idx = int(c)
+                f = func_list[idx][1]
+                f(controller)
+            except Exception, e:
+                print e
+                print traceback.format_exc()
+                running = False
+
 
 if __name__ == '__main__':
     controller = None
     try:
         controller = DemoController(config)
-        controller.calibrate_csys()
+        controller.calibrate_cylinder_sys()
-        #controller.movel(config.R_LVL0, 0, config.Z_MIN + config.BLOCK_DIM)
-        #check_edges(controller)
-        #test_movec_hax(controller)
-        #check_theta_edges(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()
 
+    tests = [
+        ("Check z-levels", check_z_levels),
+        ("Test unitary moves.", test_unitary_moves),
+        ("Test multi-directional moves", test_multi_moves),
+        ("Check loop constraints", check_loop_constraints),
+        ("Test URScript's movec", test_movec)
+    ]
+    unit_tests(tests, controller)
+
     time.sleep(1)
     if controller:
         controller.cleanup()
-    sys.exit(1)
+    sys.exit(0)

ur5controller.py

@@ -24,8 +24,8 @@ class DemoController(object):
 		self.quadrant = config.TABLE_QUADRANT
 		self.phi = self.quadrant*math.pi/2
 		self.cyl_offset = config.TABLE_ORIGO_OFFSET
-		self.cyl_ox = self.cyl_offset*math.cos(self.phi)
+		self.cyl_ox = self.cyl_offset[0]*math.cos(self.phi)
-		self.cyl_oy = self.cyl_offset*math.sin(self.phi)
+		self.cyl_oy = self.cyl_offset[1]*math.sin(self.phi)
 
 		# Home pose params
 		self.j_home = config.JOINTS_HOME
@@ -72,12 +72,12 @@ class DemoController(object):
 	def move_to_home_pose(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)
+			self.robot.movej(self.j_home, acc=0.5, vel=0.4, radius=0, wait=True, relative=False)
 			pose = self.robot.getl()
 			pose[0] = self.r_home*math.cos(self.phi+self.theta_home) + self.cyl_ox
 			pose[1] = self.r_home*math.sin(self.phi+self.theta_home) + self.cyl_oy
 			pose[2] = self.z_home
-			self.robot.movel(pose, acc=0.1, vel=0.2, radius=0, wait=True, relative=False)
+			self.robot.movel(pose, acc=0.15, vel=0.2, radius=0, wait=True, relative=False)
 			self.current_cyl = (self.r_home, self.theta_home, self.z_home)
 			if self.debug:
 				print "Initial cylinder coords: %s" % str([('%.2f' % i) for i in self.current_cyl])
@@ -94,8 +94,7 @@ class DemoController(object):
 		csys.orient.rotate_zb(self.phi)
 		self.robot.set_csys("csys", csys)
 		self.trans_base = self.robot.get_transform()
-		self.cyl_ox = self.cyl_offset
+		self.cyl_ox, self.cyl_oy = self.cyl_offset
-		self.cyl_oy = 0
 
 
 	def set_cylinder_coords(self):
@@ -175,7 +174,7 @@ class DemoController(object):
 		return move_list
 
 
-	def movec_hax(self, r, theta, z, blend_last=0, wait=False):
+	def movec_hax(self, r, theta, z, wait=False):
 		"""movec with linearized arc."""
 		# manual acc/vel
 		move_list = self.linearize_arc(r, theta, z)
@@ -188,43 +187,43 @@ class DemoController(object):
 
 	def move(self, vec, t=0):
 		"""Move in vec direction. Assumes that |v| != 0"""
-		vr, vtheta, vz = vec
+		dr, dtheta, dz = vec
 		r, theta, z = self.current_cyl
 
-		if vtheta == 0:
+		if dtheta == 0: #linear move
-			# linear move
+			if dz == 0:
-			if vz == 0:
+				move = self.cylinder2cartesian(r+dr, theta, z) + [self.acc, self.vel, 0]
-				move = 
-
 			else:
-			# arc move
+				if dr == 0:
+					move = self.cylinder2cartesian(r, theta, z+dz) + [self.acc, self.vel_z, 0]
+				else:
+					move = self.cylinder2cartesian(r+dr, theta, z+dz*(self.vel_z/self.vel)) + [self.acc, self.vel, 0]
+			self.exec_move(move)
 
+		else: #arc move
+			dtheta = dtheta/r # angle of arc segment with length 1
+			if dz == 0:
+				moves = self.linearize_arc(r+dr, theta+dtheta, z)
+			else:
+				moves = self.linearize_arc(r+dr, theta+dtheta, z+dz*(self.vel_z/self.vel))
+			self.exec_moves(moves)
 
-		if vr != 0 or (vr != 0 and vtheta != 0):
+		if t != 0:
-
-
-		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)
+			self.robot.stopl(acc=self.stop_acc)
+
 
 	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.move([-1*i for i in vec], t=self.t_force)
 			self.set_current_cyl()
 			return
 
 		self.set_current_cyl()
-		vr, vtheta, vz = vec
+		dr, dtheta, dz = vec
 		r, theta, z = self.current_cyl
 
 		# move?
@@ -234,30 +233,27 @@ class DemoController(object):
 			prev_vec = (0,0,0)
 			return
 
-		# check bounds
+		# check projected constraints
-		rnext = r + (vr*self.vel*dt)
+		rnext = r + (dr*self.vel*dt)
 		if rnext < self.r_min or rnext > self.r_max:
-			vr = 0
+			dr = 0
-		thetanext = theta + (vtheta*self.vel*dt) # this is angular speed, but the diff is not significant
+		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:
-			vtheta = 0
+			dtheta = 0
-		znext = z + (vz*self.vel*dt)
+		znext = z + (dz*self.vel_z*dt)
 		if znext < self.z_min or znext > self.z_max:
-			vz = 0
+			dz = 0
-		vec = (vr, vtheta, vz)
+		vec = (dr, dtheta, dz)
 
-		# command change
+		# change move
 		if vec != self.prev_vec:
-			# from stand still
+			if sum(map(abs, self.prev_vec)) == 0: #from still
-			if sum(map(abs, self.prev_vec)) == 0:
 				self.move(vec)
 				self.prev_vec = vec
 				self.t_ch = 0
-
+			elif not self.t_ch: #from another command
-			# from another command
-			elif not self.t_ch:
 				self.t_ch = time.time()
-				self.robot.stopl(acc=self.chcmd_decel)
+				self.robot.stopl(acc=self.stop_acc)
 			elif time.time() - self.t_ch > self.t_chcmd:
 				self.move(vec)
 				self.prev_vec = vec
@@ -278,8 +274,10 @@ class DemoController(object):
 
 	def blocklevel2move(self, r_lvl, theta, z_lvl, acc=0.5, vel=0.1, r=0):
 		# loop acc/vel
-		return self.cylinder2pose(self.r_lvl0 - r_lvl*self.block_dim, theta, self.z_lvl0 + z_lvl*self.block_dim)
+		r = self.r_lvl0 - r_lvl*self.block_dim
-
+		z = self.z_lvl0 + z_lvl*self.block_dim
+		move = self.cylinder2cartesian(r, theta, z) + [acc, vel, r]
+		return move
 
 
 	def pick_block(self, r_lvl, theta, z_lvl, acc, vel):
@@ -295,12 +293,11 @@ class DemoController(object):
 		r_lvl starts at 0, which correspond to the outer edge of the table.
 		z_lvl starts at 0, which corrsepond to table level.
 		"""
-		# loop acc/vel
+		r = self.r_lvl0 - r_lvl*self.block_dim
-		r_target = self.r_lvl0 - r_lvl*self.block_dim
+		z = self.z_lvl0 + z_lvl*self.block_dim
-		z_target = self.z_lvl0 + z_lvl*self.block_dim
+		p1 = self.cylinder2cartesian(r+self.r_pick_init, theta+self.theta_pick_init, z+self.z_pick_init)
-		p1 = self.cylinder2pose(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin)
+		p2 = self.cylinder2cartesian(r, theta, z)
-		p2 = self.cylinder2pose(r_target, theta, z_target)
+		p3 = self.cylinder2cartesian(r+self.r_pick_end, theta+self.theta_pick_end, z+self.z_pick_end)
-		p3 = self.cylinder2pose(r_target + self.r_boffset, theta + self.theta_boffset, z_target + self.z_boffset)
 		move_list = [
 			p1 + [self.acc, vel, 0.005],
 			p2 + [self.acc, vel, 0],
@@ -311,8 +308,7 @@ class DemoController(object):
 
 	def place_block(self, r_lvl, theta, z_lvl, acc, vel):
 		"""Reverse move of pick_block."""
-		# loop acc/vel
+		moves = self.pick_block(r_lvl, theta, z_lvl, acc, vel)
-		moves = self.pick_block(r_lvl, theta, z_lvl, vel)
 		moves.reverse()
 		return moves