From f4ca1805f8324504967eb46cd0aa036690dcfee8 Mon Sep 17 00:00:00 2001 From: Michael Soukup Date: Wed, 6 Aug 2014 00:42:00 +0200 Subject: [PATCH] CONSTRAINTS FIXED. FUCKYEAH --- config.py | 14 ++++++--- config.pyc | Bin 1759 -> 1749 bytes demo.py | 78 +++++++++++++++++++++++++++++++--------------- ur5controller.py | 62 +++++++++++++++++++++++++----------- ur5controller.pyc | Bin 12502 -> 12828 bytes 5 files changed, 106 insertions(+), 48 deletions(-) diff --git a/config.py b/config.py index 62559e5..6fa8d66 100644 --- a/config.py +++ b/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 -R_MIN = 0.5 -R_MAX = 0.8 +# Constraints are relative to the cylinder coordinate system. +# Because of delay and fluctuations of parameters in the system, +# 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 diff --git a/config.pyc b/config.pyc index b195b2a2735820d096514da5c806c2ed0abcd5ba..12e199d8b00324706346f29fe634dca125aee333 100644 GIT binary patch delta 294 zcmXwyJ5Iw;5Jk^Rkco&)5W;z}^C3V;!Y4p#3JP{Hayb?fn?Pa%2zdkSAq|L*Jy3^& z0?{IRo(GuX>df!nW&9dP^{;sCQtqSA8;2xl18JiP;!Qhf8%a@*WTsu4_s|Z~M^j`# zu|ZQ?B(hzh(U4*t%_y2|dD*e9``7K7#hzJ@&>pgn&NF0;_K`_-9R4aFFH;J*ZiRqN zl3&z8wKymroSz+{AvA=@5gH-KrZaSioEV>)R~9bL%6Ipi=65gO(|mbxh1JJDyp@l& UTQ^^v*S%ZhD|yYWmQT_62UBD>>Hq)$ delta 304 zcmXXBr5S+*91tjMg!}0Ep4^ae_5B0RPptHB##KL4>NQ|97z{G!Gu9Av>KtV&| z@37WH8)z+UcyD!z$?Wdz%zN+=_})*j^&7tUtEUK51s6dYY;`Q0fUBSstU)#5y6hX^ z7}NwOpgou!lsQC1Ehk0o!)$|7m@+Dw4rHqRPi=`fl;{q)4(ft4n|B0mfR3Hn{<$1Y zCosT$XbW}`{Yh@RLoc5yJ?w*Rzb&Bw*ar=TPr)tFndo!LimfBdztvj;!RKf^d39Kh j#+QPxd{KB-7xODmtJ0=5UEe)Cn)%JfZ9#eY%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 [2]", + "Start loop [1]: four blocks high", + "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.") diff --git a/ur5controller.py b/ur5controller.py index 916e222..7a2d804 100644 --- a/ur5controller.py +++ b/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 and force > self.force_constraint: - self.move([-1*i for i in vec], t=self.t_force) - self.set_current_cyl() - return + if self.force_mon: + 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) + 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) - prev_vec = (0,0,0) + self.robot.stopl(acc=self.stop_acc) + self.prev_vec = (0,0,0) return - # check projected constraints - rnext = r + (dr*self.vel*dt) - if rnext < self.r_min or rnext > self.r_max: + # Projected position does not work. Fuck this, add empirical offsets instead + #curr_d = (r**2 + (r*theta)**2 + z**2)**0.5 + #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 or thetanext > self.theta_max: + if (thetanext < self.theta_min and dtheta < 0) or (thetanext > self.theta_max and dtheta > 0): + 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 or znext > self.z_max: + if (znext < self.z_min and dz < 0) or (znext > self.z_max and dz > 0): + 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: diff --git a/ur5controller.pyc b/ur5controller.pyc index 656e61d3df1ec271ad843d97b4343ba53d84259a..a4af8ef75293ae2b69bcd8e05b4386894d650eb0 100644 GIT binary patch delta 2843 zcmZ`*TX2&_6yE*+T$;3L(_3h1fs~7l($GL_ZJ}7Ch=8TgztWLQ3DAVn&};HjZizu_ zg$I~%SZ5sM>g~mG)DlDnQD%J685u`k7$3aP=!5#=@aBw{^PN8}g(@XqzTLBX&e?Nz z&&loG=OPaMj>p~7J>mFHlQo&2wQiZt5lJD!H6r3*5)=^|lO-Z4MXZ*2i1||H%b3?O zuV?-+^9JURFmGhu#C$pP70jENuVnrx^Oh|Xd;~v01Km-s~IYpbjtZ4}2VZfl;SF}|QWj-mI zEeodn85kOk8eQtQ;!W~!W<|+*89ao=xq!ofBLsmQFy7-LhCSi_zJy5|CrTfZNqFlu z-6V=0iuQydPduAh==(udY@tbxl+i`3J%AV)IEZmj%`2PFfib2z2}zggD)Y;oq_xTZ z0!^IEykGW#CcD)@MMxf07b@n?Jlm~`IkS(^S`Ko;j1HTy)N>#n566w;O0R5lglWQn z)<$)zvPhm$mnwg6wY)-7jWYoDZ=BfOGtY1j>y82T1H!D)IX7Qk&iLkjAvgJGb|Zzr z-#6$VjrRB>eer0I**7$}#0tb>lD7vu*lPs>43kDR;6XqNzz&V8_5M%;#~xmiLcTtx zsnIABH(sJIiyyh)(WB~Je{1nkTJRBl67va!Rv?QU%3f8K&Z7~WXr)i$0O_X38xtZv z#)KZMCL&Upx)>8OqKR_6ZJRLdEVhX;$;2V{+a~wiD=kgwyuv9WI{qhhv6l#!gjkM9 zk4Y(E(nSWyf?aeDT0U44R5EtxB4raPyU1#C^Nd5JoFbJY%v@pSu|HCbY4C@P?AJv6 zZ@gk?_K5wui2uQy&Xb=lOtAv8DASV6McfdEJp$&FRJLq*^(C`XE|GMKq+LXEFrrmd z+sJtrM!6zMV$$ywbOGY661&JWg>k3MiV8U_C*6QbgLPIBEZq!&)?{ycqFoENGWZ{C8WC-z-vq_R4*^6YTG>+ z@kgk((o7#7xm7hT5^Lu&(ff4MmCr^$zSz=RZtKbqUOwN_`{y&U8>TwZGNU`GuNTZu z=h9XaPrWy4hRvSgF!v~98_gI?0Ve?q04Ns5BESd$^@%C~3d8_^59}T`LXeyX1OfGc z20$ZVIiMNvC}5co&B8bfT1&v^68k z(Lt*Tyn*RA0URF0FWVDTUG-uaR^jR{S*5O4pXgYPRjUAN00#*I=!RCyv=fVm`$zk^ z4^si9UkN;1j3r$2Uj*YWNcVxVz%Av|K#2@yz6rderT@c#u@)=W0X6{I0PTRy0O+0; z?8}%N24p6smgOmr``1fvi_;HP6dy>Z_X4d1y(-|LT2> zkFYZ6mF=oMxJP!ViC{!-QNATRJlxFiV`yL~5>xn1tOoV?sTkFT82?^=_W=W@Dc)5q@8t&6}v#n|Q^XgI5)p^<+93MCUg delta 2518 zcmaJ@Yitx%6h3$M)!la6ZP{)MEp;hUwv=tR(n4#cJj$yArI}h|Ny}1p2HAGEyX;KY zQg>Y=R56ekxSAm1D?a|9Cb$|U3i3-6V~oZ}{ADo4Kj;q=e)vbD=bWifL2YKf{q8-F zd+xdSoas63+i|Duy4Uk*{RQXuidbI!Nf{Fb7vun3gd9R76{HFXdCn>7?N%BlZop?ZAsQ66mm5=zAmX4w%5>}iU|DT1VYqH07;6@RE&A}Vf#lT2lGby#}* zd&Ep}ufJb}A0p*af*FD%2%!>UY~w~oHqk`fFwyXf>Q&-ljMis*P@>Q1u_%d+M~i)d ztD^2coa79t!^j3gPDGV9qGn}pZ6l{eT@WKlhUIi^P>h|r+U=Ob@rmNsweKlnRGzGh zh^)L+*KqsJey&@(q=34dVZ+qZCM^`3&Sat)^>Nu)zstFgT%*%2`9^(}I3qu*|E1T8 ziB3^HL%`7;Lw0k+w*^Ofw3Ip_Q}w-aWrJ5-D0Vh{E()u0b~o-l7@rDe^;l4gXY`mE zPffK3HyOrsQa6I;p>Z>q%jSa1)(0b2URuysvBeWg0LEZs*X94ouIVK!DRRVc8-Kb+RaO+6?-zOyD68lTdFnz^AlPF*JcC;q7~ARCykA=$j5v9KH`RJz6L3Ivnx zEw}8j67HVr^T@0sG`V;WrZO>I<<|5fDN&KCFA?0*6C={u zq<4LX5@QVTEGjmxdP~{=FV57hq_~}+pI|4!Zh}1oEeN49V$|@| zB0-Aa9RiYCCuXfFBjLrOaN{GDj>jgW2NJ2+rO#CS}%+;zyOTMvjq`CF?ddgN%^DDsPKn;ng^(-;I?B|KSP4F%O?MD58;1U6?tzJP0 z;ky8gCOGwLv)tcaRnzVExy#%hx2LS6thCG8*E{*Yi?i~4`{BO;Z1m`~