From 816ec92a813f841c98645d9ecfc53a9324ef12a9 Mon Sep 17 00:00:00 2001
From: Michael Soukup <mic.soukup@gmail.com>
Date: Wed, 6 Aug 2014 00:54:37 +0200
Subject: [PATCH] Force monitor working just fine.

---
 config.py         |   8 ++++----
 config.pyc        | Bin 1749 -> 1763 bytes
 ur5controller.py  |   4 +---
 ur5controller.pyc | Bin 12828 -> 12837 bytes
 4 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/config.py b/config.py
index 6fa8d66..6e1c2d4 100644
--- a/config.py
+++ b/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,
diff --git a/config.pyc b/config.pyc
index 12e199d8b00324706346f29fe634dca125aee333..52247a81dbd46438740b5b1259c44fe64382b498 100644
GIT binary patch
delta 287
zcmXwzJ5Iw;5Jit2<08l)K^)sj%qMmdhw$G(8a9CFrlJ@tf`XDY&|0Pdc0f%<N6P}}
zky28!0bZWWEk<|l)f~-NunhdxZ_swHD}T0~AuZHHJTynv&{1_Ub;i}*!&!)Q(Jm69
z5fY;@lAsCFLwiWyCIhsOr0AfYIO%ypxlSjNeaa0qZMLCBWcDCOcaV_<jnUou-pMcb
z2qT$N!tExyMc6o#$8vxgoWaTr^^rq6kI*4<Z2ifqHlKCm6m_^NCiRc=RNNH6;^ldz
Y&v|3s|IO=sVQQoA`q9naKHSjz10eW4o&W#<

delta 252
zcmXYpJ5Iw;6hzN+j1_?l62$Q@29h|05dK?$4IsLyuNw+V)<A2yVISE8JD^8ONy!EX
zPic<k&Rm^!{1r#N-+1>yZL~ZZqaGHb5SySKY}!69T;0CD-}TWDOHk^WVMCN-DJncm
z-;b~iRaoB5U3Fj3>@i3+rrF2J?KSbQQ?ID85t?BII>V~&&DBrGB&ilO@N$41l5Eqa
w&vb#Ec#)P^gf2a=uv2vH^UeRf5e{^VIiAmF-H-d|+92;%LFv9%pFtJ=0g_ZN0{{R3

diff --git a/ur5controller.py b/ur5controller.py
index 7a2d804..ea857b4 100644
--- a/ur5controller.py
+++ b/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)
diff --git a/ur5controller.pyc b/ur5controller.pyc
index a4af8ef75293ae2b69bcd8e05b4386894d650eb0..cb0fe7a0c032fd74adb2f09a60dae295762d87bc 100644
GIT binary patch
delta 758
zcmZvazfTlV5XZl>Z*SQ<IpxP5o2Z1GoP=mpz#kNLQec7|4GBaN@?vq9K)B;#Ox{Xy
z1)(zDv$RuVt638|DE$YhDebI`Rz{sSdrOYs<`whi`{pw<JM&h5(1H3<E}cC696YEF
zzEFSx>OgbPKCa$f+=Eu)@e%D5dRB)}Niz2UwC|2lc&4DU7a=-C<Tyoi0MdaytS;>O
zXn1IPWJow=G!&XY#U|7*LJx?2iDQU10e!*U*UQiWmQ4V|nL<QyQiS2B1?Zx?m0&`b
zUzgmCV_4?YLuhapi{zp>GqBs*pq@62hL7fcuJzj-^IS?F!X~F=&@hs~Arc>4l;L#j
z91;3^a#aYubwoXBRN&uoiQ$>;`e*vF&_{CmC=&UHNkQn+ay^ol<hiag`c=<lQbp(s
zxy1BSo8hybD=y066(pgQt{84#PTpT)8qeU1cV-43CF=h(80?=x|KQs8&tx2CJf-Y-
zEq=&r!1Efzc3ZhJ#?ZQ_c4E|Hw*{Hx{O1#Gx9wK#iq{rQ)Q53fz{8&|+k@J?|Chh?
zd+im`Wt&XR5v8A|mOPqBe@|a0x|v?Pv88A(?cU<;dt%?*{z}Vs?an(|>CU{SRok8U
KMUU+Mto;Y_`IsR9

delta 768
zcmZvayK59t6vn?ZcV-{St|r+hRt}CqBdhDXOtBV1z(#C@nC5CR3kmzMyF<W1OF?fL
z1tHigQ%SlsY5g1g4@A()$GK-_WCdYh&YAC?-}&yG^IpHv74@yTw7LDUaw}{+Re%9v
zV4B*y;q_4}sgHS31Sz4DC`qc&$~{Km2?xG>tRW>L$EYI(NC);f9PKNNJxqVWkeq5_
ztT6p5F(Ez$U6J@S*AVRjddcNGP3Qpk4U0Z>742S9hpEB@&^4FwshaDr>n>Y{VVPU6
zV9Yldk*ng)!2awJ71}DsKBmu0Z87GWnOI-JZtiClNyQ-&A8g8qsd!E3@5s3j`s|1b
zsU`3)rNr<&cK2ucXQ8i^^cE8Nhe;rGw$I+%a`Gp+Tjlh=XL6|{^i8jjnEp3r_`u7{
z3+o*uHL<Q3Zl9gI88eM%@WK0M28UAh{~1Wvat4coo2dCR=8bYInn*n!yZGe}zdaj9
zy_PJ=P8XAja>lxt4eeHRW@B;Etj&9fd*D^c5B1%vFBhHIvr)gjfN^`l>u{lAkD}Yz
zkLV>)*S=W4Mk@QXzU5Jv1sBf~4YErcdx~yk2LoQkd-m<+6MA4DUpb_Qwmy7C+xB4i
Ko$lDzSM47|{Fg5P