diff --git a/config.py b/config.py
index 9513fce..cc2af14 100644
--- a/config.py
+++ b/config.py
@@ -14,30 +14,42 @@ USE_FORCE_MONITOR = True
 VELOCITY = 0.1
 ACCELERATION = 0.5
 DECELERATION = 0.5
+DEFAULT_LOOP_SPEED = 0.1
 
 # Block parameters
-BLOCK_DIM = 0.1 #TODO measure
+BLOCK_DIM = 0.0995 #TODO measure
 
-# Table polar parameters
-TABLE_QUADRANT = 3 # In range [0..4). Quadrant 0 is along the x-axis of the robot.
+# 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
-TABLE_EDGE_LEFT = -math.pi/4
-TABLE_EDGE_RIGHT = math.pi/4
+Z_TABLE = -0.336
 R_MIN = 0.6 # relative to TABLE center
 R_MAX = 0.9 # relative to TABLE center
-
-# Table height parameters (TODO CHECK THESE)
-Z_TABLE = -0.337
+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
+THETA_EDGE_RIGHT = math.pi/4 - math.pi/68
+R_BLOCKMOVE_MARGIN = BLOCK_DIM + 0.05 # r margin before and after pick/place
+THETA_BLOCKMOVE_MARGIN = 0 # theta margin before and after pick/place
+Z_BLOCKMOVE_MARGIN = 0 # z margin before and after pick/place
+R_BLOCKMOVE_OFFSET = -0.01 # r start/end in place/pick. lift slightly inwards to avoid collision with other blocks
+THETA_BLOCKMOVE_OFFSET = 0 # theta start/end in place/pick
+Z_BLOCKMOVE_OFFSET = BLOCK_DIM*0.5 # z start/end in place/pick
+
 
 # Home quadrant joint configuration.
 # We need this expressed in joint space to ensure that the robot does not
 # collide with itself when moving to the base configuration.
+# This is also for fine-tuning the tool orientation
 QUADRANT_JOINT_CONFIG = [TABLE_QUADRANT*math.pi/2,
 			   	   		 -3*math.pi/4,
 			   	   		 -3*math.pi/4,
 			   	   		 -math.pi/2,
 			   	  		 -math.pi/2,
-			  	  		 math.pi]
+			  	  		 49*math.pi/50]
diff --git a/config.pyc b/config.pyc
index d1eaa97..5adbe54 100644
Binary files a/config.pyc and b/config.pyc differ
diff --git a/testcontroller.py b/testcontroller.py
deleted file mode 100755
index 37be22a..0000000
--- a/testcontroller.py
+++ /dev/null
@@ -1,85 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-
-import sys, config, math, time
-from ur5controller import DemoController
-
-
-def check_edges(controller):
-    controller.movel(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
-    print 'Right edge.'     
-    dummy = raw_input('Press any key to continue...')
-
-    controller.movel(config.R_MIN, 0, config.Z_MIN)
-    print 'Middle.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
-
-    controller.movel(config.R_MIN, config.TABLE_EDGE_LEFT, config.Z_MIN)
-    print 'Left edge.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
-
-def test_movec(controller):
-    controller.movec(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
-    print 'Right edge.'     
-    dummy = raw_input('Press any key to continue...')
-
-    controller.movec(config.R_MIN, 0, config.Z_MIN)
-    print 'Middle.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
-
-    controller.movec(config.R_MIN, config.TABLE_EDGE_LEFT, config.Z_MIN)
-    print 'Left edge.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
-
-def test_movec_hax(controller):
-    controller.movec_hax(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
-    print 'Right edge.'     
-    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.TABLE_EDGE_LEFT, 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.TABLE_EDGE_RIGHT, 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.TABLE_EDGE_LEFT, config.Z_MIN+0.1)
-    print 'Left edge.'
-    time.sleep(1)
-    dummy = raw_input('Press any key to continue...')
-
-
-
-if __name__ == '__main__':
-    controller = None
-    try:
-        controller = DemoController(config)
-        #check_edges(controller)
-        test_movec_hax2(controller)
-
-        # NEXT: test radial params and arbitrary paths.
-    except Exception, e:
-        print e
-
-    time.sleep(1)
-    if controller:
-        controller.cleanup()
-    sys.exit(1)
diff --git a/unittesting.py b/unittesting.py
new file mode 100755
index 0000000..f198ba7
--- /dev/null
+++ b/unittesting.py
@@ -0,0 +1,149 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import sys, config, math, time
+from ur5controller import DemoController
+
+
+def check_edges(controller):
+    controller.movel(config.R_MIN, config.THETA_MAX, config.Z_MIN)
+    print 'Right edge.'     
+    dummy = raw_input('Press any key to continue...')
+
+    controller.movel(config.R_MIN, 0, config.Z_MIN)
+    print 'Middle.'
+    time.sleep(1)
+    dummy = raw_input('Press any key to continue...')
+
+    controller.movel(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(controller):
+    controller.movec(config.R_MIN, config.THETA_MAX, config.Z_MIN)
+    print 'Right edge.'     
+    dummy = raw_input('Press any key to continue...')
+
+    controller.movec(config.R_MIN, 0, config.Z_MIN)
+    print 'Middle.'
+    time.sleep(1)
+    dummy = raw_input('Press any key to continue...')
+
+    controller.movec(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_hax(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)
+    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)
+    controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN)
+    print 'Theta edge right.'     
+    dummy = raw_input('Press any key to continue...')
+
+    controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_LEFT, config.Z_MIN)
+    controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_LEFT, config.Z_MIN)
+    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
+    #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 - math.pi/200#no gap
+
+    # Initial: 1x2 left side
+    #speed = 0.7
+    speed = 0.05
+
+    moves = []
+    moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed)
+    moves += controller.place_block(0, dtheta_lvl0/2, 0, speed)
+    moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 0, speed)
+    moves += controller.place_block(0, -dtheta_lvl0/2, 0, speed)
+    controller.movels(moves, wait=True)
+    moves.reverse()
+    controller.movels(moves, wait=True)
+    # Shit is smooth!
+
+
+def pick_place_small_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: 2x2 on each side
+
+
+    controller.blocklvl2pose(r_lvl, theta, z_lvl) + [acc, vel, rad]
+
+    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__':
+    controller = None
+    try:
+        controller = DemoController(config)
+        #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)
+
+        # NEXT: test radial params and arbitrary paths.
+    except Exception, e:
+        print e
+
+    time.sleep(1)
+    if controller:
+        controller.cleanup()
+    sys.exit(1)
diff --git a/ur5controller.py b/ur5controller.py
index 998fc66..7e69d20 100644
--- a/ur5controller.py
+++ b/ur5controller.py
@@ -5,8 +5,9 @@ class UR5Exception(Exception):
 
 class DemoController(object):
 	"""Implements control of the UR5 in cylinder coordinates (corresponding to attached table).
-	   -Checks bounds of the arc section
-	   -Methods for placing blocks
+	   
+	-Checks bounds of the arc section
+	-Methods for placing blocks
 	"""
 
 	def __init__(self, config):
@@ -17,7 +18,12 @@ class DemoController(object):
 		except Exception, e:
 			raise UR5Exception('Robot initiation failed: %s' % str(e))
 
-		# Set cylinder parameters
+		# Control params
+		self.vel = config.VELOCITY
+		self.acc = config.ACCELERATION
+		self.dec = config.DECELERATION
+
+		# Cylinder params
 		self.quadrant = config.TABLE_QUADRANT
 		self.phi = self.quadrant*math.pi/2
 		self.cyl_offset = config.TABLE_ORIGO_OFFSET
@@ -26,17 +32,25 @@ class DemoController(object):
 
 		self.r_min = config.R_MIN
 		self.r_max = config.R_MAX
-		self.theta_min = config.TABLE_EDGE_LEFT
-		self.theta_max = config.TABLE_EDGE_RIGHT
+		self.theta_min = config.THETA_MIN
+		self.theta_max = config.THETA_MAX
 		self.z_min = config.Z_MIN
 		self.z_max = config.Z_MAX
 
+		# Block params
+		self.block_dim = config.BLOCK_DIM
+		self.r_lvl0 = config.R_LVL0
+		self.z_lvl0 = config.Z_LVL0
+		self.r_bmargin = config.R_BLOCKMOVE_MARGIN
+		self.theta_bmargin = config.THETA_BLOCKMOVE_MARGIN
+		self.z_bmargin = config.Z_BLOCKMOVE_MARGIN
+		self.r_boffset = config.R_BLOCKMOVE_OFFSET
+		self.theta_boffset = config.THETA_BLOCKMOVE_OFFSET
+		self.z_boffset = config.Z_BLOCKMOVE_OFFSET
+		
+		# Home quadrant joint config
 		self.j_home = config.QUADRANT_JOINT_CONFIG
 
-		self.vel = config.VELOCITY
-		self.acc = config.ACCELERATION
-		self.dec = config.DECELERATION
-
 		# Move to reference coordinate system base
 		self.set_pose_home()
 
@@ -49,9 +63,8 @@ class DemoController(object):
 		self.cyl_oy = 0
 		
 
-
 	def set_pose_home(self):
-		"""Move to "home" configuration. The resulting pose ensures correct orientation."""
+		"""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()
@@ -65,9 +78,7 @@ class DemoController(object):
 
 
 	def cylinder2pose(self, r, theta, z):
-		"""Translate table cylinder coordinates to robot cartesian pose.
-		   (r, theta) is given in TABLE coordinates.
-		"""
+		"""Translate table cylinder coordinates to robot cartesian pose."""
 		trans = math3d.Transform(self.trans_base) #deep copy
 		trans.orient.rotate_zb(theta)
 		trans.pos = math3d.Vector(r*math.cos(theta) + self.cyl_ox,
@@ -75,53 +86,120 @@ 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 movel(self, r, theta, z):
+		"""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.current_cyl = (r, theta, z)
 
 
 	def movec(self, r, theta, z):
-		#Note: r should be the same.
+		"""Circular move - uses URScript built-in function.
+
+		Unfortunately, this doesn't work very well: The tool orientation at the target pose 
+		is not right. URScript specifies that orientation in pose_via is not considered, however,
+		it SHOULD consider the orientation of the end pose.
+		"""
 		curr_theta = self.current_cyl[1]
 		pose_via = self.cylinder2pose(r, curr_theta+(theta-curr_theta)/2, z)
 		pose = self.cylinder2pose(r, theta, z)
 		self.robot.movec(pose_via, pose, acc=self.acc, vel=self.vel, radius=0, wait=True)
 		self.current_cyl = (r, theta, z)
 
+
+	def movels(self, move_list, wait=True):
+		"""Concatenate several movel commands and applies a blending radius.
+		move_list = pose + [acc, vel, radius]
+		"""
+		header = "def myProg():\n"
+		end = "end\n"
+		template = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})\n"
+		prog = header + ''.join([template.format(*m) for m in move_list]) + end
+		self.robot.send_program(prog)
+		if not wait:
+			return None
+		else:
+			self.robot.wait_for_move()
+			return self.robot.getl()
+
+
 	def movec_hax(self, r, theta, z):
-		"""Since movec is unreliable in reaching the end pose: linearize the arc."""
+		"""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.
+
+		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
+		"""
+
+		move_list = []
+		step = math.pi/41
 		curr_r, curr_theta, curr_z = self.current_cyl
 		dtheta = theta - curr_theta
-		if dtheta > 0:
-			theta_incr = math.pi/41
-		else:
-			theta_incr = -math.pi/41
-		if abs(dtheta) < abs(theta_incr):
-			self.movel(r, theta, z)
-			return
 
-		pose_list = []
-		segments = int(math.floor(dtheta/theta_incr))
-		r_incr = (r-curr_r)/(segments+1)
-		z_incr = (z-curr_z)/(segments+1)
-		for i in range(1, segments+1):
-			pose_list.append(self.cylinder2pose(curr_r + i*r_incr,
-												curr_theta + i*theta_incr,
-												curr_z + i*z_incr))
-		pose_list.append(self.cylinder2pose(r, theta, z))
+		segments = max(int(round(abs(dtheta)/step)), 1)
+		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])
 
 		#debug
 		if self.debug:
-			print "pose list for movec_hax"
-			for p in pose_list:
+			print "move list for movec_hax"
+			for p in move_list:
 				print [('%.3f' % i) for i in p]
 
-		self.robot.movels(pose_list, acc=self.acc, vel=self.vel, radius=0.05, wait=True)
+		self.movels(move_list, wait=True)
 		self.current_cyl = (r, theta, z)
 
 
+	def pick_block(self, r_lvl, theta, z_lvl, speed):
+		"""This function concatenates 3 moves:
+
+		1. Move the tool in front of the block.
+		2. Move the tool into the hole.
+		3. Lift the block.
+
+		Here, we assume that the tool is nearby the block. Bringing the tool towards the block is
+		not the responsibility of this function.
+
+		r_lvl starts at 0, which correspond to the outer edge of the table.
+		z_lvl starts at 0, which corrsepond to table level.
+		"""
+		r_target = self.r_lvl0 - r_lvl*self.block_dim
+		z_target = self.z_lvl0 + z_lvl*self.block_dim
+		p1 = self.cylinder2pose(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin)
+		p2 = self.cylinder2pose(r_target, theta, z_target)
+		p3 = self.cylinder2pose(r_target + self.r_boffset, theta + self.theta_boffset, z_target + self.z_boffset)
+		# TODO take speed as arg
+		move_list = [
+			p1 + [self.acc, speed, 0.005],
+			p2 + [self.acc, speed, 0],
+			p3 + [self.acc, speed, 0.01],
+		]
+		return move_list
+
+	def place_block(self, r_lvl, theta, z_lvl, speed):
+		"""Reverse move of pick_block."""
+		# TODO take speed as arg
+		moves = self.pick_block(r_lvl, theta, z_lvl, speed)
+		moves.reverse()
+		return moves
+
+		#r_target = self.r_lvl0 + r_lvl*self.block_dim
+		#_target = self.z_lvl0 + z_lvl*self.block_dim
+
+		#self.movec_hax(r_target + self.r_boffset, theta + self.theta_boffset, z_target + self.z_boffset)
+		#self.movel(r_target, theta, z_target)
+		#self.movel(r_target - self.r_bmargin, theta - self.theta_bmargin, z_target - self.z_bmargin)
+
 
 	def cleanup(self):
 		self.robot.cleanup()
diff --git a/ur5controller.pyc b/ur5controller.pyc
index 0592f99..a6e1091 100644
Binary files a/ur5controller.pyc and b/ur5controller.pyc differ