restructure

config.py

@@ -2,71 +2,136 @@ import math
 
 DEBUG = True
 
+
 # GUI
+#
+# Frame rates should be high enough to poll keys efficiently.
 SCREEN_DIM = (800, 600)
 FONTSIZE = 24
-BG_COL = (255,255,255) #white
+BG_COL = (255,255,255)
-FONT_COL = (0,0,0) #black
+FONT_COL = (0,0,0)
-MENU_FPS = 15 # enough to poll keys efficiently
+MENU_FPS = 15
-CTR_FPS = 60
+CTR_FPS = 60 #manual control
 
-# IP of the robot.
+
+
+# NETWORK
+#
+# The IP address can be found in the PolyScope interface (tablet) of the robot.
+# SETUP Robot -> Setup NETWORK (requires password) -> IP address
 UR5_IP = "129.241.187.119"
+UR5_HOSTNAME = 'ur-2012208984' #requires dns.
 
-# 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.
+# BLOCK DIMENSION
+#
+# Blocks are cubic and the hole is centered.
+BLOCK_DIM = 0.0975 #not exactly 10cm
+
+
+
+# TABLE CONFIGURATION
+#
+# The table configuration specifies parameters for the cylinder coordinate system.
+# All table parameters are relative to the robots base coordinate system.
+#
+# TABLE_QUADRANT was added because the UR5's kinetmatics computations are dependent
+# on its own base coordinate system. For instance, quadrant 3.0 results in joint
+# violations in the original setup. Quadrant 0.0 corresponds to the table center
+# 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_Z = -0.336 #this should be fine-tuned on each setup
+TABLE_ARC = math.pi/2
+
+
+
+# HOME JOINT CONFIGURATION
+#
+# This is the joint config for the robot's initial position.
+# We need this also expressed in joint space to ensure that the robot does not
+# collide with itself when moving to the base configuration, and to ensure
+# the correct tool orientation when calibrating the reference cylinder
+# coordinate system.
+#
+# Change joints 2-6 in QUADRANT_JOINTS_HOME to tune the tool orientation.
+R_HOME = 0.5
+THETA_HOME = 0
+Z_HOME = TABLE_Z + 2*BLOCK_DIM
+JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
+			   	   		-3*math.pi/4,
+			   	   		-3*math.pi/4,
+			   	   		-math.pi/2,
+			   	  		-math.pi/2,
+			  	  		49*math.pi/50]
+
+
+
+# FORCE MONITOR
+#
+# Enables force constraints on the tool (for manual control).
+# When the force exceeds FORCE_CONSTRAINT (newton), the robot move in the direction
+# 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
 FORCE_CONSTRAINT = 60
-FORCE_T = 0.1
+T_FORCE_VIOLATION = 0.1
 
-# Control parameters.
-VELOCITY = 0.1
-ACCELERATION = 0.5
-DECELERATION = 0.5
-CHCMD_DECEL = 1.0
-CHCMD_T = 0.1 # 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
+# MANUAL CONTROL
-BLOCK_DIM = 0.0975 #TODO measure
+#
+# Parameters that specifies velocity curve and constraints of manual control.
+# Note that the angular velocity is also [m/s], not [rad/s], to ensure that the robot
+# moves with constant speed regardless of direction in the polar plane.
+# IMPORTANT: VEL_Z should be tuned to Z_MIN to avoid smashing into the table.
+VEL = 0.1 #[m/s]
+VEL_Z = 0.05 #[m/s]
+ACC = 1.0 #[m/s^2]
+STOP_ACC = 1.0 #aka deceleration
+T_DIR_CHANGE_COOLDOWN = 0.1
 
-# Table cylinder parameters
+# Constraints are relative to the cylinder coordinate system
-TABLE_QUADRANT = 0 # In range [0..4). Quadrant 0 is along the x-axis of the robot.
+R_MIN = 0.5
-TABLE_ORIGO_OFFSET = -0.25 # TODO measure
+R_MAX = 0.8
-Z_TABLE = -0.336
+THETA_MIN= -TABLE_ARC/2
-R_MIN = 0.5 # relative to TABLE center
+THETA_MAX = TABLE_ARC/2
-R_MAX = 0.8 # relative to TABLE center
+Z_MIN = TABLE_Z + 0.6*BLOCK_DIM
-THETA_MIN= -math.pi/4 # table edge left
+Z_MAX = TABLE_Z + 4.5*BLOCK_DIM #4 blocks high
-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
+# AUTO/LOOP CONTROL
-THETA_EDGE_LEFT = -math.pi/4 + math.pi/68
+#
+# When looping, the robot use the same velocity for r, theta and z.
+# Also, since this is all planned motions, we dont need to use stopl().
+# This means that the trapezoidal velocity curve has the same slope at
+# start and end of a motion. (acceleration=deceleration)
+#
+# Constraints in r and z-direction are specefied in levels to enable
+# easier construction of loops. Note that the r-levels grow inwards, and
+# the z-levels grow upwards.
+DEFAULT_LOOP_VEL = 0.1
+LOOP_VEL_MIN = 0.05
+LOOP_VEL_MAX = 0.7
+LOOP_ACC = 0.5
+
+R_LVL0 = 0.7 #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_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
 
+# Offsets when picking (and placing) blocks.
+# The place-block move is the reverse of the pick-block move,
+# such that the start/end offsets for pick-block are the end/start offsets
+# for place-block
+R_STARTPICK_OFFSET = -1.3*BLOCK_DIM
+THETA_STARTPICK_OFFSET = 0
+Z_STARTPICK_OFFSET = 0
+R_ENDPICK_OFFSET = -0.01 #lift slightly inwards to avoid collision with adjacent blocks
+THETA_ENDPICK_OFFSET = 0
+Z_ENDPICK_OFFSET = BLOCK_DIM*0.5
 
-
-# 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,
-			  	  		 49*math.pi/50]

ur5controller.py

@@ -20,27 +20,34 @@ class DemoController(object):
 		except Exception, e:
 			raise UR5Exception('Robot initiation failed: %s' % str(e))
 
-		# Control params
+		# Cylinder coordinate system params
-		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
 		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_oy = self.cyl_offset*math.sin(self.phi)
 
+		# Home pose params
+		self.j_home = config.JOINTS_HOME
+		self.r_home = config.R_HOME
+		self.theta_home = config.THETA_HOME
+		self.z_home = config.Z_HOME
+
+		# Force monitor params
+		self.force_mon = config.USE_FORCE_MONITOR
+		self.force_constraint = config.FORCE_CONSTRAINT
+		self.t_force = config.T_FORCE_VIOLATION
+
+		# Manual control velocity params
+		self.vel = config.VEL
+		self.vel_z = config.VEL_Z
+		self.acc = config.ACC
+		self.stop_acc = config.STOP_ACC
+		self.t_chcmd = config.T_DIR_CHANGE_COOLDOWN
+		self.prev_vec = (0,0,0)
+		self.t_ch = 0
+
+		# Manual control constraints
 		self.r_min = config.R_MIN
 		self.r_max = config.R_MAX
 		self.theta_min = config.THETA_MIN
@@ -48,41 +55,40 @@ class DemoController(object):
 		self.z_min = config.Z_MIN
 		self.z_max = config.Z_MAX
 
-		# Block params
+
+		# Block move 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.r_pick_init = config.R_STARTPICK_OFFSET
-		self.j_home = config.QUADRANT_JOINT_CONFIG
+		self.theta_pick_init = config.THETA_STARTPICK_OFFSET
+		self.z_pick_init = config.Z_STARTPICK_OFFSET
+		self.r_pick_end = config.R_ENDPICK_OFFSET
+		self.theta_pick_end = config.THETA_ENDPICK_OFFSET
+		self.z_pick_end = config.Z_ENDPICK_OFFSET
 
 
-	def set_pose_home(self):
+	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)
 			pose = self.robot.getl()
-			pose[0] = self.r_min*math.cos(self.phi) + self.cyl_ox
+			pose[0] = self.r_home*math.cos(self.phi+self.theta_home) + self.cyl_ox
-			pose[1] = self.r_min*math.sin(self.phi) + self.cyl_oy
+			pose[1] = self.r_home*math.sin(self.phi+self.theta_home) + self.cyl_oy
-			pose[2] = self.z_min
+			pose[2] = self.z_home
 			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)
+			self.current_cyl = (self.r_home, self.theta_home, self.z_home)
 			if self.debug:
-				print "initial cyl coords: %s" % str([('%.2f' % i) for i in self.current_cyl])
+				print "Initial cylinder 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):
+	def calibrate_cylinder_sys(self):
 		"""Calibrate the reference cylinder coordinate system."""
 		# Move to reference coordinate system base
-		self.set_pose_home()
+		self.move_to_home_pose()
 		# Set reference coordinate system parameters
 		csys = math3d.Transform()
 		csys.orient.rotate_zb(self.phi)
@@ -90,29 +96,9 @@ 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_cylinder_coords(self):
-	def set_freedrive(self, mode):
-		"""Set UR5 to freedrive mode."""
-		self.robot.set_freedrive(mode)
-
-
-	def cylinder2pose(self, r, theta, z):
-		"""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,
-								  r*math.sin(theta) + self.cyl_oy,
-								  z)
-		return trans.pose_vector.tolist()
-
-	def set_current_cyl(self):
 		p = self.robot.getl()
 		x = p[0] - self.cyl_ox
 		y = p[1] - self.cyl_oy
@@ -122,14 +108,36 @@ class DemoController(object):
 		self.current_cyl = (r, theta, z)
 
 
-	def movel(self, r, theta, z, wait=True):
+	def cylinder2cartesian(self, r, theta, z):
-		"""Linear move."""
+		"""Translate table cylinder coordinates to reference cartesian coordinates."""
-		pose = self.cylinder2pose(r, theta, z)
+		trans = math3d.Transform(self.trans_base) #deep copy
-		self.robot.movel(pose, acc=self.acc, vel=self.vel, radius=0, wait=wait, relative=False)
+		trans.orient.rotate_zb(theta)
-		self.current_cyl = (r, theta, z)
+		trans.pos = math3d.Vector(r*math.cos(theta) + self.cyl_ox,
+								  r*math.sin(theta) + self.cyl_oy,
+								  z)
+		return trans.pose_vector.tolist()
 
 
-	def movec(self, r, theta, z):
+	def exec_move(self, move, wait=False):
+		"""Execute a linear move. move_list = pose + [acc, vel, radius]"""
+		prog = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})".format(*move)
+		self.robot.send_program(prog)
+		if wait:
+			self.robot.wait_for_move()
+
+
+	def exec_moves(self, move_list, wait=False):
+		"""Executes a list of linear moves. 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 wait:
+			self.robot.wait_for_move()
+
+
+	def movec(self, r, theta, z, wait=False):
 		"""Circular move - uses URScript built-in function.
 
 		Unfortunately, this doesn't work very well: The tool orientation at the target pose 
@@ -137,37 +145,21 @@ class DemoController(object):
 		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_via = self.cylinder2cartesian(r, curr_theta+(theta-curr_theta)/2, z)
-		pose = self.cylinder2pose(r, theta, z)
+		pose = self.cylinder2cartesian(r, theta, z)
-		self.robot.movec(pose_via, pose, acc=self.acc, vel=self.vel, radius=0, wait=True)
+		self.robot.movec(pose_via, pose, acc=self.acc, vel=self.vel, radius=0, wait=wait)
-		self.current_cyl = (r, theta, z)
 
 
-	def movels(self, move_list, wait=True):
+	def linearize_arc(self, r, theta, z, resolution=math.pi/41, blend=0.03):
-		"""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 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
+		by linearizing the arc into segments with length given in resolution, and use
 		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
 		"""
 		move_list = []
-		step = math.pi/41
+		step = resolution
 		curr_r, curr_theta, curr_z = self.current_cyl
 		dtheta = theta - curr_theta
 
@@ -177,85 +169,40 @@ class DemoController(object):
 		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)
+			pose = self.cylinder2cartesian(curr_r + i*r_incr, curr_theta + i*theta_incr, curr_z + i*z_incr)
-			move_list.append(pose + [acc or self.acc, vel or self.vel, 0.03])
+			move_list.append(pose + [self.acc, self.vel, blend])
-		move_list.append(self.cylinder2pose(r, theta, z) + [acc or self.acc, vel or self.vel, blend_last])
+		move_list.append(self.cylinder2cartesian(r, theta, z) + [self.acc, self.vel, 0]) #dont  blend last
 		return move_list
 
-	def movec_hax(self, r, theta, z, blend_last=0, wait=True):
+
+	def movec_hax(self, r, theta, z, blend_last=0, wait=False):
 		"""movec with linearized arc."""
+		# manual acc/vel
 		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=wait)
+		self.exec_moves(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:
-
-		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)
-		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."""
-		moves = self.pick_block(r_lvl, theta, z_lvl, speed)
-		moves.reverse()
-		return moves
-
-
-	def construct_moves(self, task_list, speed):
-		"""Construct moves from a list of tasks"""
-		pass # wait with implementing this
-
-
-	def is_looping(self, dt, threshold=0.001):
-		"""Polls the robot for change in pose vector. Blocks for dt seconds."""
-		pose0 = self.robot.getl()
-		time.sleep(dt)
-		pose1 = self.robot.getl()
-		v = [pose1[i]-pose0[i] for i in range(len(pose0))]
-		if self.debug:
-			_sum = sum(map(abs, v))
-			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):
+		"""Move in vec direction. Assumes that |v| != 0"""
 		vr, vtheta, vz = vec
 		r, theta, z = self.current_cyl
 
+		if vtheta == 0:
+			# linear move
+			if vz == 0:
+				move = 
+
+		else:
+			# arc move
+
+
+		if vr != 0 or (vr != 0 and vtheta != 0):
+
+
 		if vr != 0:
 			self.movel(r+vr, theta, z, wait=False)
 		elif vtheta != 0:
@@ -268,7 +215,6 @@ class DemoController(object):
 			time.sleep(t)
 			self.stopl(acc=self.chcmd_decel)
 
-
 	def update(self, vec, dt):
 		"""Update movements based on vec and dt"""
 
@@ -320,5 +266,67 @@ class DemoController(object):
 				self.prev_vec = vec
 				self.t_ch = 0
 
+
+	def is_looping(self, dt, threshold=0.001):
+		"""Polls the robot for change in pose vector. Blocks for dt seconds."""
+		pose0 = self.robot.getl()
+		time.sleep(dt)
+		pose1 = self.robot.getl()
+		v = [pose1[i]-pose0[i] for i in range(len(pose0))]
+		if self.debug:
+			_sum = sum(map(abs, v))
+			print "Velocity vector, dt=%.2f: %s. SUM %.3f" % (dt, str([('%.2f' % i) for i in v]), _sum)
+		return sum(map(abs, v)) > threshold
+
+
+	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)
+
+
+
+	def pick_block(self, r_lvl, theta, z_lvl, acc, vel):
+		"""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.
+		"""
+		# loop acc/vel
+		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)
+		move_list = [
+			p1 + [self.acc, vel, 0.005],
+			p2 + [self.acc, vel, 0],
+			p3 + [self.acc, vel, 0.01],
+		]
+		return move_list
+
+
+	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, vel)
+		moves.reverse()
+		return moves
+
+
+	def construct_moves(self, task_list, acc, vel):
+		"""Construct moves from a list of tasks"""
+		pass # wait with implementing this
+
+
+	def set_freedrive(self, mode):
+		self.robot.set_freedrive(mode)
+
 	def cleanup(self):
 		self.robot.cleanup()