def onStart(self): """ Start the JoyApp and the simulation """ self.ani = AnimatorPlan(self, self._animation) self.t, self.q, self.y, self.p, self.l = [], [], [], [], [] self.T0 = self.now self.ani.start()
def onStart(self): AnimatorPlan(self, _animation).start()
class ArmAnimatorApp(JoyApp): def __init__(self, wlc, Tws2w, Tp2ws, *arg, **kw): if 'cfg' not in kw: kw['cfg'] = {} kw['cfg'].update(windowSize=[1200, 800]) JoyApp.__init__(self, *arg, **kw) self.arm = ArmSim(wlc) self.Tp2w = dot(Tws2w, Tp2ws) # World to paper self.Tw2p = inv(self.Tp2w) # Paper with origin at origin self.paper_p = asarray([[8, 11, -1 / 2.56, 1]]) * xyzCube self.paper_w = dot(self.paper_p, self.Tp2w.T) # Workspace box L = 12 # Workspace in arm units self.ws_w = dot(asarray([[L, L, L, 1]]) * xyzCube, Tws2w.T) self.ws_p = dot(self.ws_w, self.Tw2p.T) # Projection onto paper in world self.Tprj = dot(self.Tp2w, asarray([[1, 1, 0, 1]]).T * self.Tw2p) # World to relative paper (i.e. paper is unit cube) self.Tw2rp = self.Tw2p / self.paper_p[-1][:, newaxis] def _integrate(self): last = self.T0 dt = 0.1 qi = None ti = None while True: # Run in units of dt now = self.now h = now - last if h < dt and ti: yield continue last = now ti, qi, yi = self.arm.step(h) # Speedup ratio into simulation time # Store simulation time-step self.t.append(ti) self.q.append(qi) self.y.append(yi) pen = self.arm.getTool(qi) self.p.append(pen) # Line marking point is lowpass version of pen if self.l: self.l.append(self.l[-1] * 0.6 + pen * 0.4) else: self.l.append(pen) def _show(self, fvp): """ Wrapper for show(), to allow subclasses to override """ fvp.cla() fvp.set(xticks=[], yticks=[]) return self.show(fvp) def show(self, fvp): ti = self.t[-1] qi = self.q[-1] pen = self.p[-1] li = self.l[-1] # self.arm.plot3D(self.arm.at(qi),fvp) sk = self.arm.getSkel(self.arm.at(qi)).T fvp.plot3D(sk[0], sk[1], sk[2], lw=3, marker='o', color='#808080') # Find current pen point in paper coordinates qq = dot(self.Tw2rp, pen) # If in sheet if (qq[0] > 0) and (qq[0] < 1) and (qq[1] > 0) and (qq[1] < 1): if qq[2] > 1: # Pressed too far in lt = dict(marker='o', color='r') elif qq[2] > 0: # Drawing lt = dict(marker='.', color='g') else: # Above sheet lt = dict(marker="+", ms=15, color="m") else: # Not above or below sheet lt = dict(marker="+", color="b") # Project onto paper and bring back to world coordinates wp = dot(self.Tprj, pen) fvp.plot3D([wp[0]], [wp[1]], [wp[2]], **lt) wl = dot(self.Tprj, li) fvp.plot3D([wl[0]], [wl[1]], [wl[2]], 'xk') fvp.xyz.set_title("t = %.2f, n=%d" % (ti, len(self.t))) plotVE(fvp, self.paper_w, iCube, 'g--', alpha=0.3) plotVE(fvp, self.paper_w[::2, :], iFace, 'g-') plotVE(fvp, self.ws_w, iCube, 'k:') def _animation(self, fig): fig.clf() last = self.T0 fvp = FourViewPlot(fig, f=80.) sim = self._integrate() dt = 0.1 while True: next(sim) # allow simulation to run # Make sure graphics run at most 33% of the time now = self.now if now - last < dt * 2: yield continue last = now # Draw graphics, measure execution time tic = time() self._show(fvp) dt = time() - tic # Display progress progress(("(%4.2f) " % dt) + " ".join([ "%15s" % ("%d/%d/%s" % (m.get_pos(), m.get_goal(), (str( int(m.get_temp())) if m.get_error() is None else "*ERR*"))) for m in self.arm ]), sameLine=True) yield def onStop(self): # We need this kind of import so as not to mess up JoyApp plotting from pylab import figure, savefig # Collect pen motions and tool tip motions p = asarray(self.p).T l = asarray(self.l).T t = asarray(asarray(self.t) * 100, int) # Convert to paper coordinates qq = dot(self.Tw2rp, p) lp = dot(self.Tw2p, l) # Prepare output as integers: # time, x, y, depth pout = c_[t, asarray(lp[:2] * 100, int).T, asarray(qq[2] * 100, int)] with open("result-%d.csv" % time(), "w") as rf: for pp in pout: rf.write(repr(list(pp))[1:-1] + "\n") # Draw on "paper" fig = figure(2) fig.clf() ax = fig.gca() # projected track is pale blue ax.plot(lp[0], lp[1], '-b', alpha=0.2) # Find when pen was up, and remove those points up = (qq[2] < 0) | (qq[0] < 0) | (qq[0] > 1) | (qq[1] < 0) | (qq[1] > 1) lp[:, up] = nan # Draw in black ax.plot(lp[0], lp[1], '-k', lw=2) # Find when pen was jammed in paper bad = qq[2] > 1 if any(bad): # If found, highlight in red lp[:, ~bad] = nan ax.plot(lp[0], lp[1], '.-r', lw=3, alpha=0.5) # Draw the frame of the paper fr = self.paper_p[::2, [0, 1]][[0, 1, 3, 2, 0]].T ax.plot(fr[0], fr[1], 'b--', lw=2) ax.axis('equal') ax.grid(1) savefig("result-%d.png" % time(), dpi=300) def onStart(self): """ Start the JoyApp and the simulation """ self.ani = AnimatorPlan(self, self._animation) self.t, self.q, self.y, self.p, self.l = [], [], [], [], [] self.T0 = self.now self.ani.start() def onEvent(self, evt): """ The keyboard row: asdfghjkl moves your motors one way The keyboard row: zxcvbnm,. moves your motors the other way 'q' will quit and store the results in a results.png image and results.csv file. """ # Ignore everything except keydown events if evt.type != KEYDOWN: return # Punt exit keys if evt.key in {K_q, K_ESCAPE}: return JoyApp.onEvent(self, evt) # row of 'a' on QWERTY keyboard increments motors p = "asdfghjkl".find(evt.unicode) if p >= 0: self.arm[p].set_pos(self.arm[p].get_goal() + 500) return # row of 'z' in QWERTY keyboard decrements motors p = "zxcvbnm,.".find(evt.unicode) if p >= 0: self.arm[p].set_pos(self.arm[p].get_goal() - 500) return