def on_release(self, event):
self.press = None
if event.inaxes == self.confax:
print "calling rrt path"
min_dist, path = self.rrt.path((event.xdata, event.ydata))
print "path: ", path
self.arm.trajectory = path
self.arm.trajectoryLoc = 0
elif event.inaxes == self.armax:
eetargetp = event.xdata, event.ydata
print "armax", eetargetp
mind = None
mincspace = None
minreal = None
for realpt, cspacep in self.arm.ikmap.iteritems():
print "cspacep", cspacep
d = dist(realpt[0:2], eetargetp)
if mind == None or d < mind:
mind = d
mincspace = cspacep
minreal = realpt
print "target:", eetargetp
print "best ik real", minreal
print "best ik cspace", mincspace
min_dist, path = self.rrt.path((mincspace[0], mincspace[1]))
self.arm.trajectory = path
self.arm.trajectoryLoc = 0
def add_dot(self, coordinates, radius, pxl):
x_c, y_c = coordinates
for x in range(self.clipx(x_c - radius), self.clipx(x_c + radius)):
for y in range(self.clipy(y_c - radius), self.clipy(y_c + radius)):
if rrt.dist((x_c, y_c), (x, y)) < radius:
self.new_img.putpixel((x, y), pxl)
def add_dot(self, coordinates, radius, pxl):
x_c, y_c = coordinates
pxl = np.array(pxl)
for x in range(self.clipx(x_c - radius - 1),
self.clipx(x_c + radius + 1)):
for y in range(self.clipy(y_c - radius), self.clipy(y_c + radius)):
d = rrt.dist((x_c, y_c), (x, y))
if d >= radius:
continue
cur_pxl = np.array(self.new_img.getpixel((x, y)))
true_pxl = np.array(self.img.getpixel((x, y)))
a = min(4 * float(radius - d) / radius, 1.0)
# if self.is_unfilled(x,y):
# a = .5
new_pxl = a * pxl + (1 - a) * cur_pxl
# b = .95
# new_pxl = b*new_pxl + (1-b)*true_pxl
new_pxl = [int(v) for v in new_pxl]
self.new_img.putpixel((x, y), tuple(new_pxl))
if d < float(radius) / 4:
self.prev_fill_radius[x][y] = radius
def add_line(self, endpoints):
cur, end = endpoints
delta = (end - cur) / np.linalg.norm(end - cur)
while rrt.dist(cur, end) > 1:
# update pixel
x, y = self.to_pxl(cur)
pxl = self.img.getpixel((x, y))
self.new_img.putpixel((x, y), pxl)
cur = cur + delta
def add_stroke(self, endpoints, stroke_width):
cur, end = endpoints
delta = (end - cur) / np.linalg.norm(end - cur)
x, y = self.to_pxl(cur)
pxl = self.img.getpixel((x, y))
while rrt.dist(cur, end) > 1:
# update pixel
self.add_dot(cur, stroke_width, pxl)
# self.new_img.putpixel((x,y), pxl)
cur = cur + delta
def add_line_until_occupied(self, endpoints):
cur, end = endpoints
delta = (end - cur) / np.linalg.norm(end - cur)
num_updated = 0
x, y = self.to_pxl(cur)
pxl = self.img.getpixel((x, y))
while num_updated == 0 or rrt.dist(cur, end) > 1:
# update pixel
x, y = self.to_pxl(cur)
existing_pxl = self.new_img.getpixel((x, y))
already_occ = (existing_pxl[0] != 0 or existing_pxl[1] != 0)
# self.new_img.putpixel((x,y), pxl)
num_updated += 1
cur = cur + delta
if already_occ:
return num_updated
return num_updated