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