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 ori_pen_to_pattern(r,t,s):
return hom(rotation_matrix_rot_tilt_skew(r,t,s))
# init - misc
num_points_side = 20
# rot, tilt, skew
# debug purposes
rot = zeros(n.round((num_points_side**2)**(1/3.0)))
tilt = linspace(-20, 20, n.round((num_points_side**2)**(1/3.0)))
skew = linspace(0, 45, n.round((num_points_side**2)**(1/3.0)))
# best for now
rot = zeros(n.round((num_points_side**2)**(1/3.0)))
tilt = linspace(-20, 20, n.round((num_points_side**2)**(1/3.0)))
skew = linspace(-90, 90, n.round((num_points_side**2)**(1/3.0)))
angles = mat(list(it.product(rot, tilt, skew)))
# init - tool
joints = (0,0,0,0,0,0)
dh_params['tool'] = hom(0,0,0,
[0.005,0.005,0.233])
dh_params['tool'][:3,:3] = ori(50,-45,0).dot(dh_params['tool'][:3,:3])
dh_params['tool'][:3,:3] = dh_params['tool'][:3,:3].dot(ori(0,180,90)).dot(xyz(5,-10,0))
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,
# init - misc
num_points_side = 20
# rot, tilt, skew
# debug purposes
rot = zeros(n.round((num_points_side**2)**(1/3.0)))
tilt = linspace(-20, 20, n.round((num_points_side**2)**(1/3.0)))
skew = linspace(0, 45, n.round((num_points_side**2)**(1/3.0)))
# best for now
rot = zeros(n.round((num_points_side**2)**(1/3.0)))
tilt = linspace(-20, 20, n.round((num_points_side**2)**(1/3.0)))
skew = linspace(-90, 90, n.round((num_points_side**2)**(1/3.0)))
angles = mat(list(it.product(rot, tilt, skew)))
# init - tool
joints = (0,0,0,0,0,0)
dh_params['tool'] = hom(0,0,0,
[0.005,0.005,0.233])
tool = dh_params['tool']
geom = {
'wobjs': [],
'pen_oris': [],
'poses': [],
'flanges': [],
}
e = lambda mag: mat([uniform(-1,1)*mag for _ in range(9)]).reshape(3,3)
xy = it.product(linspace(-0.3,0.3, 9), linspace(-0.3,0.3, 9))
for x,y in xy:
w = ori_pen_to_pattern(uniform(-90,90),uniform(0,40),uniform(-90,90))
w[:3,:3] = w[:3,:3] + e(2e-3)
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'])
##sys.path.append('../int/djikstra/')
##from graph import shortestPath as shortest_path
numpy.set_printoptions(precision=2)
numpy.set_printoptions(suppress=True)
# generate angles
rot = numpy.linspace(0,180)
tilt = numpy.linspace(-40,40)
skew = numpy.linspace(0,0)
angles = nzip(rot, tilt, skew)
if __name__ == '__main__':
dh_table['tool'] = hom(0,0,0,[0.0,0.0,0.1])
wobj = hom(-90,180,0,[0.3,0,0.5])
# generate a curve in the last global robot-frame
curve = create_circle_curve(diameter=0.3)
oris = orientation_frames(angles)
frames = attach_frames(oris, curve)
# tansform frames - paper -> robot
trajectory = attach_to_base_frame(wobj, *frames)
result = inverse_kinematics_curve(trajectory)
path = find_single_path(result)
print path
for count in xrange(0,50,11):
def main():
# init - random generator
set_state(rand_state)
# init - tool
joints = (0, 0, 0, 0, 0, 0)
dh_params['tool'] = hom(0, 0, 0, [0.005, 0.005, 0.233])
tool = dh_params['tool']
tool[:3, :3] = rot_mat_rts(0, -20, 90).dot(tool[:3, :3])
e = lambda mag: mat([uniform(-1, 1) * mag for _ in range(9)]).reshape(3, 3)
#-------------- prepare mesurements -----------------
old_time = None
res = []
for iteration in xrange(100): # standard value is 100
print "iteration {} of 100".format(iteration + 1)
old_time = time.time()
geom = {
'wobjs': [],
'pen_oris': [],
'poses': [],
'flanges': [],
}
# 20x20 = 400 measurements
xy = it.product(linspace(-0.3, 0.3, 20), linspace(-0.3, 0.3, 20))
for x, y in xy:
w = ori_pen_to_pattern(uniform(-90, 90), uniform(0, 40),
uniform(-90, 90))
pen = w[:3, :3] = w[:3, :3] + e(2e-3)
wobj = hom(0, 180, -90, [x, y, 0])
wobj[:3, :3] = wobj[:3, :3] + e(1e-1)
pose = wobj.dot(w.T)
try:
geom['flanges'].append(flange(pose))
geom['wobjs'].append(wobj)
geom['poses'].append(pose)
geom['pen_oris'].append(pen)
except:
pass
## print len(geom['poses'])
rx, ry, rz = [], [], []
num_meas = range(4, 300)
for k in num_meas:
A = mat(geom['poses'])[:k, :3, :3]
B = mat(geom['flanges'])[:k, :3, :3]
r1, (u, s, v) = solve_ori(A, B)
r1 = r1.T
R = tool[:3, :3]
xang = acos(R[:, 0].dot(r1[:, 0])) * 180 / pi
yang = acos(R[:, 1].dot(r1[:, 1])) * 180 / pi
zang = acos(R[:, 2].dot(r1[:, 2])) * 180 / pi
rx.append(xang)
ry.append(yang)
rz.append(zang)
res.append((rx, ry, rz))
##print len(res)
print "- time: {}\n".format(time.time() - old_time)
#-------------- prepare results / plots -------------
figpath = r"C:\Users\***REMOVED***\Dropbox\exjobb\results\calibrationalg_res\penori"
res = mat(res)
rx = res[:, 0, :]
ry = res[:, 1, :]
rz = res[:, 2, :]
_all = [rx, ry, rz]
_titles = ["x-axis", "y-axis", "z-axis"]
print matplotlib.get_backend()
for o, t in zip(_all, _titles):
clf()
plot(num_meas, n.max(o, axis=0), label="max")
plot(num_meas, n.mean(o, axis=0), label="mean")
plot(num_meas, n.min(o, axis=0), label="min")
legend(fontsize=LEGEND_SIZE)
grid()
xlim([num_meas[0], num_meas[-1]])
xticks(fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
#xticks(arange(4,300,(300)/12))
xlabel("Number of measurements", fontsize=LABEL_SIZE)
ylabel("Error [deg]", fontsize=LABEL_SIZE)
title(t, fontsize=TITLE_SIZE)
# QT backend
manager = plt.get_current_fig_manager()
manager.window.showMaximized()
savefig(path.join(figpath, "{}.png".format(t.replace("-", "_"))),
bbox_inches="tight")
def ori_pen_to_pattern(r, t, s):
return hom(rot_mat_rts(r, t, s))