def moveto(target):
'''moves the arm to a target coordinate'''
pathresolution = 2
path = pp.plan(target, pathresolution)
anglesbuffer = []
for coords in path:
angles = ik.calcInverse(coords)
anglesbuffer.append(angles)
for angles in anglesbuffer:
if angles:
s1.movea(angles[0])
s2.movea(angles[1])
s3.movea(angles[2])
time.sleep(max(s1.diff, s2.diff, s3.diff) / 3000.0)
def drawfig(fig):
table = -163
lift = 10
pathresolution = 0.5
lines = fig.lines
targets = []
paths = []
anglesbuffer = []
targets.append([lines[0][0], lines[0][1], table + lift])
for i in range(len(lines) - 1):
targets.append([lines[i][0], lines[i][1], table])
targets.append([lines[i][2], lines[i][3], table])
if lines[i + 1][0] != lines[i][2] or lines[i + 1][1] != lines[i][3]:
targets.append([lines[i][2], lines[i][3], table + lift])
targets.append([lines[i + 1][0], lines[i + 1][1], table + lift])
targets.append([lines[len(lines) - 1][0], lines[len(lines) - 1][1], table])
targets.append([lines[len(lines) - 1][2], lines[len(lines) - 1][3], table])
targets.append([lines[len(lines) - 1][2], lines[len(lines) - 1][3], table + lift * 2])
for target in targets:
paths.append(pp.plan(target, pathresolution))
for path in paths:
for coords in path:
angles = ik.calcInverse(coords)
anglesbuffer.append(angles)
for angles in anglesbuffer:
if angles:
s1.movea(angles[0])
s2.movea(angles[1])
s3.movea(angles[2])
time.sleep(max(s1.diff, s2.diff, s3.diff) / 2000.0)
else:
print "invalid angle"
