comit

config.py

@@ -67,6 +67,7 @@ JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
 			   	  		-math.pi/2,
 			  	  		49*math.pi/50]
 
+<<<<<<< HEAD
 
 
 # FORCE MONITOR
@@ -122,6 +123,45 @@ 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
+=======
+# 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.

demo.py

@@ -163,14 +163,11 @@ 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))
 
-            vec = tuple(vec)
             self._disptxt([
                 "Keyboard control",
                 "Use arrow keys, [w], and [s]",
                 " ",
-                "move vec: %s" % str(vec),
-                "prev vec: %s" % str(self.controller.prev_vec),
-                "cylinder coords: %s" % str([('%.2f' % i) for i in self.controller.current_cyl])
+                "move vec: %s" % str(vec)
             ])
 
             dt = 1.0/config.CTR_FPS

ur5controller.py

@@ -206,8 +206,7 @@ class DemoController(object):
 		if vr != 0:
 			self.movel(r+vr, theta, z, wait=False)
 		elif vtheta != 0:
-			# set end point edge
-			self.movec_hax(r, vtheta*self.theta_max, z, wait=False)
+			self.movec_hax(r, theta+vtheta, z, wait=False)
 		elif vz != 0:
 			self.movel(r, theta, z+vz, wait=False)
 
@@ -216,7 +215,7 @@ class DemoController(object):
 			self.stopl(acc=self.chcmd_decel)
 
 	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 and force > self.force_constraint:
@@ -225,33 +224,31 @@ class DemoController(object):
 			return
 
 		self.set_current_cyl()
-		r, theta, z = self.current_cyl
 		vr, vtheta, vz = vec
-
-		# check bounds
-		rnext = r + (vr*self.vel*dt)
-		if (rnext < self.r_min and vr < 0) or (rnext > self.r_max and vr > 0):
-			vr = 0
-		thetanext = theta + (vtheta*self.vel*dt) # this is angular speed, but the diff is not significant
-		if (thetanext < self.theta_min and vtheta < 0) or (thetanext > self.theta_max and vtheta > 0):
-			vtheta = 0
-		znext = z + (vz*self.vel*dt)
-		if (znext < self.z_min and vz < 0) or (znext > self.z_max and vz > 0):
-			vz = 0
-		vec = (vr, vtheta, vz)
+		r, theta, z = self.current_cyl
 
 		# move?
 		if sum(map(abs, vec)) == 0:
 			if vec != self.prev_vec:
-				# stop
 				self.robot.stopl(acc=self.chcmd_decel)
-			self.prev_vec = (0,0,0)
+			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