CONSTRAINTS FIXED. FUCKYEAH

config.py

@@ -74,7 +74,7 @@ JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
 # 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
+USE_FORCE_MONITOR = True
 FORCE_CONSTRAINT = 60
 T_FORCE_VIOLATION = 0.1
 
@@ -92,12 +92,16 @@ ACC = 1.0 #[m/s^2]
 STOP_ACC = 1.0 #aka deceleration
 T_DIR_CHANGE_COOLDOWN = 0.1
 
-# Constraints are relative to the cylinder coordinate system
+# Constraints are relative to the cylinder coordinate system.
-R_MIN = 0.5
+# Because of delay and fluctuations of parameters in the system,
-R_MAX = 0.8
+# the calculation of projected positions are not correct.
+# Therefore, an empirical offset is added in controller.update().
+# Consider this before changing control speed
+R_MIN = 0.49
+R_MAX = 0.75
 THETA_MIN= -TABLE_ARC/2
 THETA_MAX = TABLE_ARC/2
-Z_MIN = TABLE_Z + 0.6*BLOCK_DIM
+Z_MIN = TABLE_Z + 0.5*BLOCK_DIM
 Z_MAX = TABLE_Z + 4.5*BLOCK_DIM #4 blocks high
 
 

demo.py

@@ -26,7 +26,7 @@ class UR5Demo(object):
 
         self.clock = pygame.time.Clock()
         self.sig_quit = False
-        self.loop_speed = config.DEFAULT_LOOP_SPEED
+        self.loop_speed = config.DEFAULT_LOOP_VEL
         self.controller = None
 
     def _disptxt(self, s_arr):
@@ -54,7 +54,7 @@ class UR5Demo(object):
 
     def go_home_pos(self):
         self._disptxt(["Going to home configuration..."])
-        self.controller.movel(config.R_MIN, 0, config.Z_MIN)
+        self.controller.movel(config.R_HOME, config.THETA_HOME, config.Z_HOME)
 
     def freedrive(self):
         self.controller.set_freedrive(True)
@@ -81,10 +81,10 @@ class UR5Demo(object):
                     self.sig_quit = True
             if pressed[pygame.K_PLUS] or pressed[pygame.K_KP_PLUS]:
                 self.loop_speed += incr
-                self.loop_speed = min(self.loop_speed, config.LOOP_SPEED_MAX)
+                self.loop_speed = min(self.loop_speed, config.LOOP_VEL_MAX)
             elif pressed[pygame.K_MINUS] or pressed[pygame.K_KP_MINUS]:
                 self.loop_speed -= incr
-                self.loop_speed = max(self.loop_speed, config.LOOP_SPEED_MIN)
+                self.loop_speed = max(self.loop_speed, config.LOOP_VEL_MIN)
             elif pressed[pygame.K_ESCAPE] or self.sig_quit:
                 running = False
 
@@ -97,7 +97,7 @@ class UR5Demo(object):
 
     def run_loop1(self):
         loopcount = 0
-        self._disptxt(["Running loop 1", "Loopcount: %d" % loopcount])
+        self._disptxt(["Running loop \"four blocks high\"", "Loopcount: %d" % loopcount])
         time.sleep(1)
         
         # initiate loop, poll for robot move change and keypress
@@ -116,20 +116,29 @@ class UR5Demo(object):
 
     def run_loop2(self):
         loopcount = 0
-        self._disptxt(["Running loop 2", "Loopcount: %d" % loopcount])
+        self._disptxt(["Running loop \"small pyramid\"", "Loopcount: %d" % loopcount])
         time.sleep(1)
-        pass
+        
+        # initiate loop, poll for robot move change and keypress
+        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
+            self.clock.tick(config.MENU_FPS)
+
+        # Set to default pos before return?
 
 
     def loop_init(self, block_init_msg, loopnum):
         msg = [
-            "Initital block configuration for loop %d:" % loopnum,
-            block_init_msg,
-            " ",
             "Current loop speed is %.2f m/s" % self.loop_speed,
             "Start loop? [ENTER]"
         ]
-        self._disptxt(msg)
+        self._disptxt(block_init_msg + msg)
         running = True
         while running:
             pressed = pygame.key.get_pressed()
@@ -150,6 +159,7 @@ class UR5Demo(object):
             self.clock.tick(config.MENU_FPS)
 
     def keyboard_control(self):
+        t = time.time()
         running = True
         while running:
             pressed = pygame.key.get_pressed()
@@ -163,21 +173,26 @@ class UR5Demo(object):
             for m in (self.kb_map[key] for key in self.kb_map if pressed[key]):
                 vec = map(sum, zip(vec, m))
 
-            self._disptxt([
+            new_t = time.time()
-                "Keyboard control",
+            dt = max(new_t-t, 1/config.CTR_FPS)
-                "Use arrow keys, [w], and [s]",
+            t = new_t
-                " ",
-                "move vec: %s" % str(vec)
-            ])
 
-            dt = 1.0/config.CTR_FPS
             try:
-                self.controller.update(vec, dt)
+                self.controller.update(tuple(vec), dt)
             except Exception, e:
                 self._disptxt(["Error: %s" % e])
                 time.sleep(2)
                 running = False
 
+            f = self.controller.zforce
+            self._disptxt([
+                "Keyboard control",
+                "Use arrow keys, [w], and [s]",
+                " ",
+                "move vec: %s" % str(vec),
+                "dt: %.3f" % dt,
+                "force z: %s %.1f" % (" " if f>0 else "", f)
+            ])
             self.clock.tick(config.CTR_FPS)
 
 
@@ -186,8 +201,8 @@ class UR5Demo(object):
             "Go to [h]ome position",
             "[f]reedrive mode",
             "Adjust loop [s]peed",
-            "Start loop [1]",
+            "Start loop [1]: four blocks high",
-            "Start loop [2]",
+            "Start loop [2]: small pyramid",
             "Manual [c]ontrol",
             "[q]uit"
         ]
@@ -212,12 +227,25 @@ class UR5Demo(object):
                 self._disptxt(menu)
 
             elif pressed[pygame.K_1]:
-                _instr = "2x2 blocks on left side"
+                _instr = [
+                    "Initital block configuration for loop \"four blocks high\":",
+                    "On each side",
+                    "    (r0, edge, z0)",
+                    "    (r1, edge, z0)",
+                    " "
+                ]
                 self.loop_init(_instr, 1)
                 self._disptxt(menu)
 
             elif pressed[pygame.K_2]:
-                _instr = "2x2 blocks on left side, 2x2 blocks on right side"
+                _instr = [
+                    "Initital block configuration for loop \"small pyramid\":",
+                    "On each side",
+                    "    (r0, edge, z0)",
+                    "    (r1, edge, z0)",
+                    "    (r0, edge, z1)",
+                    " "
+                ]
                 self.loop_init(_instr, 2)
                 self._disptxt(menu)
 
@@ -240,9 +268,9 @@ class UR5Demo(object):
         try:
             self.controller = DemoController(config)
             self.controller.set_freedrive(False)
-            msg.append("Calibrating cylinder coordinates...")
+            msg.append("Calibrating cylinder coordinate system...")
             self._disptxt(msg)
-            self.controller.calibrate_csys()
+            self.controller.calibrate_cylinder_sys()
         except Exception, e:
             return self.quit(error=("Error: %s" % e))
         msg.append("Calibration done.")

ur5controller.py

@@ -45,6 +45,7 @@ class DemoController(object):
 		self.stop_acc = config.STOP_ACC
 		self.t_chcmd = config.T_DIR_CHANGE_COOLDOWN
 		self.prev_vec = (0,0,0)
+		self.zforce = 0.0
 		self.t_ch = 0
 
 		# Manual control constraints
@@ -186,10 +187,14 @@ class DemoController(object):
 
 
 	def move(self, vec, t=0):
-		"""Move in vec direction. Assumes that |v| != 0"""
+		"""Move in vec direction."""
 		dr, dtheta, dz = vec
 		r, theta, z = self.current_cyl
 
+		#if dr == 0 and dtheta == 0 and dz == 0:
+		#	self.robot.stopl(acc=self.stop_acc)
+		#	return
+
 		if dtheta == 0: #linear move
 			if dz == 0:
 				move = self.cylinder2cartesian(r+dr, theta, z) + [self.acc, self.vel, 0]
@@ -214,36 +219,57 @@ class DemoController(object):
 
 
 	def update(self, vec, dt):
-		"""Update movements based on vec (and dt?)"""
+		"""Update movements based on vec and dt"""
 
-		force = 10 # dummy
+		if self.force_mon:
-		if self.force_mon and force > self.force_constraint:
+			self.zforce = self.robot.get_tcp_force(wait=False)[2]
-			self.move([-1*i for i in vec], t=self.t_force)
+			if abs(self.zforce) > self.force_constraint:
-			self.set_current_cyl()
+				# move opposite to z force
-			return
+				dz = self.zforce/abs(self.zforce)
+				self.move((0, 0, dz), t=self.t_force)
+				self.set_cylinder_coords()
+				return
 
-		self.set_current_cyl()
+		# move?
+		self.set_cylinder_coords()
 		dr, dtheta, dz = 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)
+				self.robot.stopl(acc=self.stop_acc)
-			prev_vec = (0,0,0)
+			self.prev_vec = (0,0,0)
 			return
 
-		# check projected constraints
+		# Projected position does not work. Fuck this, add empirical offsets instead
-		rnext = r + (dr*self.vel*dt)
+		#curr_d = (r**2 + (r*theta)**2 + z**2)**0.5
-		if rnext < self.r_min or rnext > self.r_max:
+		#curr_v = abs(curr_d - self.prev_d) / dt
+		#self.prev_v = curr_v
+		#self.prev_d = curr_d
+		
+		# check projected constraints.
+		rnext = r + dr*0.031
+		thetanext = theta + dtheta*0.029
+		znext = z + dz*0.012
+		if (rnext < self.r_min and dr < 0) or (rnext > self.r_max and dr > 0):
+			r = self.r_min if dr < 0 else self.r_max
 			dr = 0
-		thetanext = theta + (dtheta*self.vel*dt) # this is angular speed, but the diff is not significant
+		if (thetanext < self.theta_min and dtheta < 0) or (thetanext > self.theta_max and dtheta > 0):
-		if thetanext < self.theta_min or thetanext > self.theta_max:
+			theta = self.theta_min if dtheta < 0 else self.theta_max
 			dtheta = 0
-		znext = z + (dz*self.vel_z*dt)
+		if (znext < self.z_min and dz < 0) or (znext > self.z_max and dz > 0):
-		if znext < self.z_min or znext > self.z_max:
+			z = self.z_min if dz < 0 else self.z_max
 			dz = 0
+
 		vec = (dr, dtheta, dz)
+		self.current_cyl = (r, theta, z)
+
+		# stop on |vec| = 0
+		if sum(map(abs, vec)) == 0:
+			if vec != self.prev_vec:
+				self.robot.stopl(acc=self.stop_acc)
+			self.prev_vec = (0,0,0)
+			return
 
 		# change move
 		if vec != self.prev_vec: