From ccb5ffb4c74acf916b83312acb59d1fb07451bdf Mon Sep 17 00:00:00 2001 From: Michael Soukup Date: Tue, 5 Aug 2014 10:01:16 +0200 Subject: [PATCH] manual controller done. Going to separate control parameters for manual mode and auto mode --- config.py | 10 +++++----- config.pyc | Bin 1557 -> 1549 bytes demo.py | 5 ++++- ur5controller.py | 35 +++++++++++++++++++---------------- ur5controller.pyc | Bin 12526 -> 12601 bytes 5 files changed, 28 insertions(+), 22 deletions(-) diff --git a/config.py b/config.py index dd03964..742f23e 100644 --- a/config.py +++ b/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 diff --git a/config.pyc b/config.pyc index 51ea7d19cc6fe62a08457eef880993d233a07ac4..cfecd6e315d0e19647ebec538fcc3bb3c5bed511 100644 GIT binary patch delta 461 zcmY*VJ5Iw;5S-XaoW#!Gj`I(HLO`PC0$d>7hz4n&kPsaWolgJ~vM8YD26U91gMyY0 zi3{+&gve|$+L^aIyZau3+rV?b>$T^*k9%*Cj6fGs1dEU{Sb|KzGQfqh6j0SDSVgml0W3~)Jy zFI_~OteBE1%mBvrhlpa%U}lgg6T+AqI%hM}xRtl~b(3D!vYm5V>q*q-j@mX5b62<9 zQ{0Ed;9T~IdR{eA1`uF)c^nh delta 469 zcmY*VyG{Z@6g|5j3+w_5%(9C-eIpho`T>+Cwsfby8xp_3XhCCZVKx@}5yr&MZ_qEW zAjHbV&L41R1cIkHx%bSyXU@#6ckelwpHgu?o839XX$Qh<`p5c8+6Fe~Wz)ZMMJVe}B_$Zx3mWh=P`M+B}B^_HUrM22`Bk(5acMKsG x#)E$HYK{HJ2aT`M|c$8SkV9g diff --git a/demo.py b/demo.py index 842c655..c74c7bb 100755 --- a/demo.py +++ b/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 diff --git a/ur5controller.py b/ur5controller.py index f17fcf9..03ec960 100644 --- a/ur5controller.py +++ b/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 diff --git a/ur5controller.pyc b/ur5controller.pyc index 8c867d28a200d5bc6da71d9e89868f1b73621cea..19f13820f40e66260fefe20fadb61eb9eef3a0b2 100644 GIT binary patch delta 873 zcmZXSzi-n}5XZk~JBjW5a2gVtfmSfEms)~1r9rjS@=v6V_0 z*t)!tB|`@W1gcsF{s86*BqSsTKxJm(AK>0=45^Tw&(HV0``o>E{?&cIp3#5j?b6=& zgUr`DeHJJ6dsiMKGT;$rNH~)aUz-M85n#?B(>+5*hC-daMmev zlt6QccrnzHpn*w|l|XAWm23rVCK5xNB%zjqa_ihK6aF@uN5qRgS;br>4wyFjoKB@t zkW@OW12 zKZh}h`0O6H39J?28F&!U@=Vr5`^Q(PGbP&MN3-&X^LQW(DC5~gljJgnYg$-AP~j3& zN&5e<&Iz+|oLQBD=hX#FB))SNFWcoV>!$o&uCjUf)a!=6yS2UJZ~6Uj=&pB%zUOZD z-5r0!?e;y_3&ow*ia2Q1#9pgbT;QaYeQD;|)C=NgYf)6%PWDjxj-+`fmfHU9B5{ek z{0%)#RcetO&blc!ufO;M1h1#> delta 838 zcmaJ;L2DCH5dLQ0W;eTLyRmLeidbWdm`IZYQ7%GhJLIVQ@?LqsJuj(RF2bqJZBHCJkF8r>2Y;HG?9e#(*#h?Jq z6$BExhm>F7i*KGamnua7oy!7Ufpr06zBZpOQk_avcRY&dy5e-qpH!dcZ+z9fK5=tG zxTcU7kbB5^Abzx9GezV9@(`Ja%tuy)CQR)JX#qnAqxX`L`oIc%YwsZj`;f9W&X+I+ zD*{pxwIF?7`hlm#O-)Ientn@wHb0ALG$5E z*>cxtFWhgr7cJMb+|b%R%bkPfGvU;{C2NwH!vmIl&ysyh=8^5XZm?6Vc~*AU`C|}s zgTFo6wd-=Mcd|Rn>;+&=+^LsfJM22rGr>49-&d0UzXon|iwXu6cGHfRpeBricFJGu z|2613vH|wKA&Va$H=C`aL8HHw*3;}<(>!Q(47Hcdr`B?LfeSj11gZ>;*qm-J9odoi zrNAqJfdKCbom&DmbEmy|HYR@g0>@Au)u=@wxlTwi&8tI<3*?cjDzr!e&0?%#$COte S(%R0GRYj^CoH8#?-v0@Fsg#=l