def onStart(self):
ArmAnimatorApp.onStart(self)
###
### TEAM CODE GOES HERE
###
#Define 4 corners in paper coordinates
square_p = asarray([
[x - 0.5 * s, y + 0.5 * s, 0, 1], #upper left
[x + 0.5 * s, y + 0.5 * s, 0, 1], #uper right
[x + 0.5 * s, y - 0.5 * s, 0, 1], #bottom right
[x - 0.5 * s, y - 0.5 * s, 0, 1] #bottom left
])
#Convert all coordinates for square to world coordinates
self.square_w = dot(square_p, self.Tp2w.T)
##Calibration
#Create calibration grid on paper. These are points to move to during calibration.
nx, ny = (2, 2) #can be adjusted to add more calibration points
x_lin = linspace(0, 8, nx)
y_lin = linspace(0, 11, ny)
xv, yv = meshgrid(x_lin, y_lin, indexing='xy')
grid = c_[xv.reshape(nx * ny, 1),
yv.reshape(nx * ny, 1),
zeros((nx * ny, 1)),
ones((nx * ny, 1))]
grid_idx = list(range(nx * ny))
for i in range(nx - (nx % 2)):
idx = nx * (2 * i + 1)
grid_idx[idx:idx + nx] = grid_idx[idx:idx + nx][::-1]
self.grid_idx = grid_idx
self.calib_grid = dot(grid, self.Tp2w.T)
self.calib_idx = 0
#if calibration file exists, load calibration array in here, and skip over next part
#also set calibrated == true so that it calculates offset
#manually delete existing calibration array file before moving on to new arena
if (os.path.exists("calib_array.npy")):
self.calib_ang = load("calib_array.npy")
print(type(self.calib_ang))
self.move.calibrated = True
print(self.calib_ang[:, :-1])
#printout of calibration matrix
# self.calib_ang = [[-1.54566359 0.00872665 -0.03543018]
# [ 0.74228853 0.511556 -2.08479579]
# [ 2.45148947 -0.61191244 2.08584299]
# [-0.80826198 0.66479591 -2.20469991]]
else:
#This is the matrix you save your angles in and use to calculate angle offset
self.calib_ang = zeros((nx * ny, len(self.arm)))
def __init__(self, Tp2ws):
###
### Student team selection -- transform from workspace coordinates to world
###
Tws2w = asarray([[1, 0, 0, 0], [0, 1, 0, -5], [0, 0, 1, -10],
[0, 0, 0, 1]])
###
### Arm specification
###
armSpec = asarray([
[0, 0.01, 1, 5, 0],
[0, 1, 0, 5, 1.57],
[0, 1, 0, 5, 0],
]).T
ArmAnimatorApp.__init__(self, armSpec, Tws2w, Tp2ws)
def show(self, fvp):
fvp.plot3D([0], [0], [0], '^k', ms=10) # Plot black triangle at origin
if self.square != []:
lt = dict(marker='+', color='r')
for point in self.square:
fvp.plot3D(point[0], point[1], point[2], **lt)
return ArmAnimatorApp.show(self, fvp)
def onStart(self):
ArmAnimatorApp.onStart(self)
###
### TEAM CODE GOES HERE
###
seg_1 = 5 # segment 1 length
seg_2 = 4 # segment 2 length
seg_3 = 2 # segment 3 length
self.square = []
self.corner_1 = []
self.corner_2 = []
self.corner_3 = []
self.corner_4 = []
self.corners = []
self.corners.append(self.corner_1)
self.corners.append(self.corner_2)
self.corners.append(self.corner_3)
self.corners.append(self.corner_4)
self.movePlan = MovePlan(app, self.arm, seg_1, seg_2, seg_3,
self.square)
def __init__(self, Tp2ws, x, y, s, **kw):
###
### Student team selection -- transform from workspace coordinates to world
###
#first three columns represent axis. Last column represents
#translation. Adjust last column to adjust workspace placement relative to arm.
#Base of arm is always at world origin
Tws2w = asarray([
[1, 0, 0, 0], #placement of workspace along x axis
[0, 1, 0, -5], #placement of workspace along y axis
[0, 0, 1, -10], #placement of workspace along z axis
[0, 0, 0, 1]
])
###
### Arm specification
###
#Each row represents an arm segment.
#First three columns represent joint rotation axis. Fourth column represents
#arm segment length. Last column represents initial arm segment angle.
armSpec = asarray([
[0, 0.02, 1, 5, 0],
[0, 1, 0, 5, 1.57],
[0, 1, 0, 5, 0],
]).T
self.armSpec = armSpec
ArmAnimatorApp.__init__(
self,
armSpec,
Tws2w,
Tp2ws,
simTimeStep=
0.25, # Real time that corresponds to simulation time of 0.1 sec
**kw)
self.idealArm = Arm(
armSpec
) #create instance of arm class without real-life properties
self.move = Move(self) #move plan
def show(self, fvp):
fvp.plot3D([0], [0], [0], '^k', ms=10) # Plot black triangle at origin
#plot square corners on paper in red
for point in self.square_w:
fvp.plot3D(*point[:-1], marker='o', color='r')
#plot steps robot arm will take from current position to next corner/calibration point in green
for i, point in enumerate(self.move.steps):
if self.currStep and self.currStep == i:
fvp.plot3D(
*point, marker='o', color='b'
) #dot that robot is currently trying to move to is blue
else:
fvp.plot3D(*point, marker='o', color='g')
#plot calibration grid points in magenta
for point in self.calib_grid:
fvp.plot3D(*point[:-1], marker='o', color='m')
return ArmAnimatorApp.show(self, fvp)
def onEvent(self, evt):
###
### TEAM CODE GOES HERE
### Handle events as you see fit, and return after
return ArmAnimatorApp.onEvent(self, evt)
def onStart(self):
ArmAnimatorApp.onStart(self)
def show(self, fvp):
fvp.plot3D([0], [0], [0], '^k', ms=10) # Plot black triangle at origin
return ArmAnimatorApp.show(self, fvp)
def onEvent(self, evt):
###
### TEAM CODE GOES HERE
### Handle events as you see fit, and return after
# Ignore everything except keydown events
if evt.type != KEYDOWN:
return
if evt.key == K_1:
print("\nset corner 1")
# x_pos has not been set
if not self.corner_1:
self.corner_1.append(self.arm[0].get_goal())
self.corner_1.append(self.arm[1].get_goal())
self.corner_1.append(self.arm[2].get_goal())
print("\n{},{},{}".format(self.corner_1[0], self.corner_1[1],
self.corner_1[2]))
return
# already set, override
self.corner_1[0] = self.arm[0].get_goal()
self.corner_1[1] = self.arm[1].get_goal()
self.corner_1[2] = self.arm[2].get_goal()
print("\n{},{},{}".format(self.corner_1[0], self.corner_1[1],
self.corner_1[2]))
return
if evt.key == K_2:
print("\nset corner 2")
# x_pos has not been set
if not self.corner_2:
self.corner_2.append(self.arm[0].get_goal())
self.corner_2.append(self.arm[1].get_goal())
self.corner_2.append(self.arm[2].get_goal())
print("\n{},{},{}".format(self.corner_2[0], self.corner_2[1],
self.corner_2[2]))
return
# already set, override
self.corner_2[0] = self.arm[0].get_goal()
self.corner_2[1] = self.arm[1].get_goal()
self.corner_2[2] = self.arm[2].get_goal()
print("\n{},{},{}".format(self.corner_2[0], self.corner_2[1],
self.corner_2[2]))
return
if evt.key == K_3:
print("\nset corner 3")
# x_pos has not been set
if not self.corner_3:
self.corner_3.append(self.arm[0].get_goal())
self.corner_3.append(self.arm[1].get_goal())
self.corner_3.append(self.arm[2].get_goal())
print("\n{},{},{}".format(self.corner_3[0], self.corner_3[1],
self.corner_3[2]))
return
# already set, override
self.corner_3[0] = self.arm[0].get_goal()
self.corner_3[1] = self.arm[1].get_goal()
self.corner_3[2] = self.arm[2].get_goal()
print("\n{},{},{}".format(self.corner_3[0], self.corner_3[1],
self.corner_3[2]))
return
if evt.key == K_4:
print("\nset corner 4")
# x_pos has not been set
if not self.corner_4:
self.corner_4.append(self.arm[0].get_goal())
self.corner_4.append(self.arm[1].get_goal())
self.corner_4.append(self.arm[2].get_goal())
print("\n{},{},{}".format(self.corner_4[0], self.corner_4[1],
self.corner_4[2]))
return
# already set, override
self.corner_4[0] = self.arm[0].get_goal()
self.corner_4[1] = self.arm[1].get_goal()
self.corner_4[2] = self.arm[2].get_goal()
print("\n{},{},{}".format(self.corner_4[0], self.corner_4[1],
self.corner_4[2]))
return
# move to programmed corners in plan
if evt.key == K_SPACE:
# self.movePlan.square = self.square
self.movePlan.start()
return
# append points and begin drawing square
if evt.key == K_5:
self.append_points(4, 3, 3, 2.0)
return
return ArmAnimatorApp.onEvent(self, evt)
def onEvent(self, evt):
###
### TEAM CODE GOES HERE
### Handle events as you see fit, and return after
#activate autonomous mode and move to a square corner by pressing 'w','e','r','t'
if evt.type == KEYDOWN:
progress('in keydown')
p = "wert".find(evt.unicode)
if p >= 0:
if self.move.isRunning():
progress('Move running!')
return
self.move.pos = self.square_w[
p] #set square corner as goal position
self.move.start()
self.move.square = True
#after each move, set the previous goal position as your new starting position
self.move.currentPos = self.move.pos
progress('Move plan started!')
return
#Press 'k' to move to grid point for calibration
if evt.key == K_k:
self.move.pos = self.calib_grid[
self.calib_idx] #set next grid point as goal position
self.move.start()
progress('Moving to calibration point')
for i, motor in enumerate(self.arm):
self.calib_ang[self.calib_idx, i] = motor.get_goal() * (
pi / 18000
) #convert angles from centidegrees to radians
#Manual adjustment before this step
#Press 'o' to calculate error between where arm moved and where it was supposed to be
if evt.key == K_o:
#Calculate error
progress('Calculate error')
if self.calib_idx < len(self.calib_ang):
real_ang = zeros(len(self.arm))
for i, motor in enumerate(self.arm):
real_ang[i] = motor.get_goal() * (pi / 18000)
self.calib_ang[self.calib_idx] = real_ang - self.calib_ang[
self.calib_idx]
self.calib_idx += 1
self.move.calDone = True
self.move.currentPos = self.move.pos
progress('Error calculation complete')
if self.calib_idx == len(self.calib_ang):
progress('Calibration_complete!')
self.move.calibrated = True
save("calib_array.npy",
self.calib_ang) #save calibration array
return
#manual movements
# row of 'a' on QWERTY keyboard increments motors
p = "asdf".find(evt.unicode)
if p >= 0:
progress('manual move')
self.arm[p].set_pos(self.arm[p].get_goal() + 300)
return
# row of 'z' in QWERTY keyboard decrements motors
p = "zxcv".find(evt.unicode)
if p >= 0:
progress('manual move')
self.arm[p].set_pos(self.arm[p].get_goal() - 300)
return
return ArmAnimatorApp.onEvent(self, evt)