def pen_pos_pattern_mm_3d(self):
"""
Pen position relative to paper origin, which
is the UL position of the pattern with crosses
used in calibration. The coordinates are converted from ad to mm.
"""
return nmap(lambda x: x + [0], self.pen_pos_pattern_mm.tolist())
def targets(xmin,xmax,ymin,ymax, num):
'''
Generates a ('num'X'num') rectangle grid of targets in the range of
[xmin, xmax], [ymin, ymax]. Depending on application these can either be
relative to a work boject or absolute.
return:
ndarray : (num**2, 4, 4)
'''
x = linspace(xmin,xmax,num)
y = linspace(ymin,ymax,num)
xy = mat(mesh(x,y)).T
res = nmap(lambda row: nmap(lambda pair: hom(0,0,0,[pair[0],pair[1],0]) ,row), xy)
a,b,c,d = res.shape
res = res.reshape((a*b,c,d))
return res
def jacobian_from_joints(*joints):
fk = forward_kinematics(*joints, **DH_TABLE)['robot_geometry_global']
if not (DH_TABLE['tool'] is None):
fk[-1] = fk[-1].dot(DH_TABLE['tool'])
fk = mat(fk)
zi = fk[:-1, :3, 2]
Jw = zi.T
on = fk[-1, :3, 3]
oi = fk[:-1, :3, 3]
odiff = on - oi
Jv = nmap(lambda x: reduce(cross, x), zip(zi, odiff)).T
J = mat([Jv, Jw])
J = J.reshape((6, 6))
return J
def calc_robot_curve_j1():
N = 100.0
T = 10.0
L = pi / 2.0
djs = linspace(0, L * 180 / pi, N)
dts = linspace(0, T, N)
djdts = djs / dts
print djdts
print djdts * dts
dt = T / N # seconds / dx
rads_per_second = L / T
tcps = mat([
calc_robot_tcp(v, 10, 20, 0, 90, 0)
for v in linspace(0, L * 180 / pi, N)
])
#w = -tcps[:,:3,2]*rads_per_second
w = mat([[0, 0, 1] * int(N)]).reshape(N, 3) * rads_per_second
print w.shape
r = tcps[:, :3, 3]
v = nmap(lambda x: reduce(n.cross, x), zip(w, r))
return tcps, nzip(v, w).reshape(N, 6), dts
tool = dh_params['tool']
# init - wobj
wobj = hom(180-45,180,0,[0.2,-0.2,-0.2])
wobj[:3,:3] = wobj[:3,:3].dot(ori(0,180,90))
# init reference
ref = zeros((3,4))
ref[:3,:3] = wobj[:3,:3]
ref[:,3] = tool[:3,3]
# init - paper targets
tars = targets(0, 100e-3,
0, 200e-3, num=num_points_side)
#tars = nmap(lambda x: x.dot(hom(ori(0,20,uniform(-180,180)))), tars)
tars = nmap(lambda x: x[0].dot(hom(ori(x[1][0], x[1][1], x[1][2]))), zip(tars,angles))
# init - global targets
global_tars = nmap(wobj.dot, tars)
# calculate flanges from global targets
flanges = nmap(flange, global_tars)
flanges = nmap(lambda x: x + err_flange(0.03), flanges)
# obtain relative pen positions from local targets
# unit: meters
penxy0 = nmap(pen_pos, tars)
# convert to 'anoto coords' with projection error
# unit: meters
penxy = nmap(lambda x: pen_pos(x) + mat((-(x[2,0]/0.70710678118654757)*0.8e-3*2,
def main():
dh_table['tool'] = hom(0, 0, 0, [0.0, 0.0, 0.1])
wobj = hom(-90, 180, 0, [0.6, 0, 0.5])
# robot movement
##robot_movement_j1, vw, dts = calc_robot_curve_j1()
##trajectory = robot_movement_j1
curve = generate_symmetric_curve()
N = len(curve)
T = 12.56
L = 2 * pi
dt = T / N # seconds / dx
rads_per_second = L / T
frame = calc_robot_tcp(0, 0, 0, 0, 90, 0)
center = frame[:3, 3]
curve = nmap(frame.dot, curve)
r = curve[:, :3] - center
frames = n.tile(frame, (50, 1)).reshape(50, 4, 4)
frames[:, :, 3] = curve
trajectory = frames
t = linspace(0, T, N)
w = mat([[0, 0, -1]] * len(trajectory)) * rads_per_second
v = nmap(lambda x: reduce(n.cross, x), zip(w, r))
w = mat([[0, 0, 0]] * len(trajectory))
vw = n.hstack((v, w))
#inverse kinematics over curve
result = inverse_kinematics_curve(trajectory)
path = find_single_path(result)
J = nmap(lambda x: jacobian_from_joints(*x), path)
Jinv = nmap(inv, J)
joint_angular_vel = nmap(lambda x: reduce(n.dot, x), zip(Jinv,
vw)) * 180 / pi
print 'Path found: \n {}'.format(path)
print 'Joint angular velocities: \n {}'.format(joint_angular_vel)
index = 0
subplots_adjust(hspace=0.55)
subplot(211)
custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
subplot(212)
custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
'r',
linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_W, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$\dot{q}_' + str(index + 1) + '$'],
loc='lower right',
fontsize=LEGEND_SIZE)
maximize_plot()
savefig(
'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q1.png',
bbox_inches='tight',
pad_inches=0)
clf()
index = 1
subplots_adjust(hspace=0.55)
subplot(211)
custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
subplot(212)
custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
'r',
linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_W, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$\dot{q}_' + str(index + 1) + '$'],
loc='upper right',
fontsize=LEGEND_SIZE)
maximize_plot()
savefig(
'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q2.png',
bbox_inches='tight',
pad_inches=0)
clf()
index = 2
subplots_adjust(hspace=0.55)
subplot(211)
custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$q_' + str(index + 1) + '$'],
loc='lower right',
fontsize=LEGEND_SIZE)
subplot(212)
custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
'r',
linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_W, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$\dot{q}_' + str(index + 1) + '$'],
loc='lower right',
fontsize=LEGEND_SIZE)
maximize_plot()
savefig(
'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q3.png',
bbox_inches='tight',
pad_inches=0)
clf()
index = 3
subplots_adjust(hspace=0.55)
subplot(211)
custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
subplot(212)
custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
'r',
linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_W, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$\dot{q}_' + str(index + 1) + '$'],
loc='upper right',
fontsize=LEGEND_SIZE)
maximize_plot()
savefig(
'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q4.png',
bbox_inches='tight',
pad_inches=0)
clf()
index = 4
subplots_adjust(hspace=0.55)
subplot(211)
custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
subplot(212)
custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
'r',
linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_W, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$\dot{q}_' + str(index + 1) + '$'],
loc='lower right',
fontsize=LEGEND_SIZE)
maximize_plot()
savefig(
'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q5.png',
bbox_inches='tight',
pad_inches=0)
clf()
index = 5
subplots_adjust(hspace=0.55)
subplot(211)
custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
subplot(212)
custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
'r',
linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel(YLABEL_W, fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
legend(['$\dot{q}_' + str(index + 1) + '$'],
loc='lower right',
fontsize=LEGEND_SIZE)
maximize_plot()
savefig(
'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q6.png',
bbox_inches='tight',
pad_inches=0)
clf()
custom_plot(nmap(det, J), 'b', linewidth=1.5)
xlabel(XLABEL, fontsize=LABEL_SIZE)
ylabel('Jacobian determinant value', fontsize=LABEL_SIZE)
xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
xlim((0, 49))
maximize_plot()
savefig(
'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\J.png',
bbox_inches='tight',
pad_inches=0)
clf()
pl = StPlot()
joints = path[13]
robot_info = forward_kinematics(*joints, **dh_table)
pl.draw_robot(robot_info['robot_geometry_global'])
pl.draw_trajectory(trajectory)
pl.draw_tool(robot_info['flange'], dh_table['tool'])
def attach_to_base_frame(base_frame, *frames):
return nmap(lambda x: matmul(base_frame, x), frames)
"angerr1":[],
"angerr2":[],
"angerr32":[],
}
for i in xrange(1000):
if i % 100 == 0:
print(i)
rts = ((rnd(-pi, pi),
rnd(-pi/2.0, pi/2.0), rnd(-pi, pi)) for _ in range(100))
hom_oris = rotation_matrices(rts)
x = ref = rts_mat(10,20,30)
y = oris = hom_oris[:,:3,:3]
z = nmap(ref.dot, oris)
y = nmap(apply_noise, y)
z = nmap(apply_noise, z)
Y = y.reshape(300,3)
Z = z.reshape(300,3)
B = Y.T.dot(Y)
C = Y.T.dot(Z)
A = solve(B,C)
U,s,V = svd(C)
S = diag(s)
def pen_pos_pattern_wobj_global(self):
return nmap(lambda x: self.yaml_wobj.dot(x),
self.pen_pos_pattern_mm_hom)
def pen_pos_pattern_mm_hom(self):
return nmap(lambda x: hom(0, 0, 0, x), self.pen_pos_pattern_mm_3d)
def flange_pos_error(self):
return nmap(norm, self.flange_pos - self.parser.robot_flange)
