manual controller done. Going to separate control parameters for manual mode and auto mode

config.py

@@ -16,7 +16,7 @@ UR5_IP = "129.241.187.119"
 # 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
+USE_FORCE_MONITOR = False
 FORCE_CONSTRAINT = 60
 FORCE_T = 0.1
 
@@ -24,8 +24,8 @@ FORCE_T = 0.1
 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
+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
@@ -39,8 +39,8 @@ BLOCK_DIM = 0.0975 #TODO measure
 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
+R_MIN = 0.5 # relative to TABLE center
+R_MAX = 0.8 # 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

demo.py

@@ -163,11 +163,14 @@ 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)
+                "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])
             ])
 
             dt = 1.0/config.CTR_FPS

ur5controller.py

@@ -259,7 +259,8 @@ class DemoController(object):
 		if vr != 0:
 			self.movel(r+vr, theta, z, wait=False)
 		elif vtheta != 0:
-			self.movec_hax(r, theta+vtheta, z, wait=False)
+			# set end point edge
+			self.movec_hax(r, vtheta*self.theta_max, z, wait=False)
 		elif vz != 0:
 			self.movel(r, theta, z+vz, wait=False)
 
@@ -269,7 +270,7 @@ 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 and force > self.force_constraint:
@@ -278,31 +279,33 @@ class DemoController(object):
 			return
 
 		self.set_current_cyl()
-		vr, vtheta, vz = vec
 		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)
 
 		# move?
 		if sum(map(abs, vec)) == 0:
 			if vec != self.prev_vec:
+				# stop
 				self.robot.stopl(acc=self.chcmd_decel)
-			prev_vec = (0,0,0)
+			self.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