Force monitor working just fine.

config.py

@@ -75,8 +75,8 @@ JOINTS_HOME = [TABLE_QUADRANT*math.pi/2,
 # 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 = True
-FORCE_CONSTRAINT = 60
-T_FORCE_VIOLATION = 0.1
+FORCE_CONSTRAINT = 40
+T_FORCE_VIOLATION = 0.3
 
 
 
@@ -88,9 +88,9 @@ T_FORCE_VIOLATION = 0.1
 # 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]
+ACC = 0.5 #[m/s^2]
 STOP_ACC = 1.0 #aka deceleration
-T_DIR_CHANGE_COOLDOWN = 0.1
+T_DIR_CHANGE_COOLDOWN = 0.05
 
 # Constraints are relative to the cylinder coordinate system.
 # Because of delay and fluctuations of parameters in the system,

ur5controller.py

@@ -225,7 +225,7 @@ class DemoController(object):
 			self.zforce = self.robot.get_tcp_force(wait=False)[2]
 			if abs(self.zforce) > self.force_constraint:
 				# move opposite to z force
-				dz = self.zforce/abs(self.zforce)
+				dz = -1*self.zforce/abs(self.zforce)
 				self.move((0, 0, dz), t=self.t_force)
 				self.set_cylinder_coords()
 				return
@@ -263,8 +263,6 @@ class DemoController(object):
 
 		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)