controller is almost up and running. Need some fixes and improvements though.

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.

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
 

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:

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 set_current_cyl(self):
-	def blocklvl2pose(self, r_lvl, theta, z_lvl):
+		p = self.robot.getl()
-		return self.cylinder2pose(self.r_lvl0 - r_lvl*self.block_dim, theta, self.z_lvl0 + z_lvl*self.block_dim)
+		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 linearize_arc(self, r, theta, z, acc=None, vel=None, blend_last=0):
-	def movec_hax(self, r, theta, z):
 		"""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(pose + [acc or self.acc, vel or self.vel, 0.03])
-		move_list.append(self.cylinder2pose(r, theta, z) + [self.acc, self.vel, 0])
+		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=wait)
-		self.movels(move_list, wait=True)
 		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()