def setUp(self):
self.dh_table = DH_TABLE
self.dh_table['tool'] = None
self.joints = (0, 0, 0, 0, 0, 0)
self.tcp = forward_kinematics(*self.joints, **self.dh_table)['tcp']
self.flange = forward_kinematics(*self.joints,
**self.dh_table)['flange']
def __calibrate(self, num_points=-1, random=False):
self.robot_infos = map(lambda x: forward_kinematics(*x, **dh_table),
self.parser.robot_joints)
fk = self.flange
# anoto coords
pentip_2d = self.parser.pen_pos
# convert to mm, first point is origin
#pentip_2d = (pentip_2d - pentip_2d[0])*0.3
f_ad = 7.0 * (25.4 / 600.0)
pentip_2d = (pentip_2d) * f_ad
if not num_points == -1:
if random:
unique_rand = list(set(randint(0, len(fk), num_points)))
fk = fk[unique_rand]
pentip_2d = pentip_2d[unique_rand]
self.geometry_info = {
'data': {
'forward_kinematics': fk,
'pentip_2d': pentip_2d
}
}
self._res, self.cond = calib.find_solution_pen_tip(
self.geometry_info, num_points)
def main():
j1 = 0
j2 = 0
j3 = 0
j4 = 0
j5 = 0
j6 = 0
joint_values = [j1, j2, j3, j4, j5, j6]
tool = hom(0, 0, 0, [0.1, 0, 0])
dh_table['tool'] = tool
info = forward_kinematics(*joint_values, **dh_table)
# multiplication from left: rotation in base to base
# multiplication from right: rotation in tool to base
tcp = info['tcp'].dot(hom(ori(-10, 30, 40)))
pose = hom(tcp[:3, :3], [0.6, 0, 0.3])
s = calc_invkin_irb140(pose, raw_solutions=True)
ik_up = forward_kinematics(*s[0], **dh_table)
ik_down = forward_kinematics(*s[5], **dh_table)
ik_up_back = forward_kinematics(*s[10], **dh_table)
ik_down_back = forward_kinematics(*s[15], **dh_table)
#plot_robot_geometry(info)
plot_robot_geometry(ik_up, 'b')
plot_robot_geometry(ik_up_back, 'b--')
plot_robot_geometry(ik_down, 'r')
plot_robot_geometry(ik_down_back, 'r--')
xlabel('x [m]', fontsize=LABEL_SIZE)
ylabel('z [m]', fontsize=LABEL_SIZE)
xticks(fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
grid()
axes().set_aspect('equal', 'datalim')
# save figure plot
figpath = r"C:\Users\***REMOVED***\Dropbox\exjobb\rapport\images"
savefig(path.join(figpath, "invkin-elbowupdown2.png"), bbox_inches='tight')
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 flange(pose):
'''
Given a pose, calulate the forward-kinematics frame obtained
from reaching the point by inverse-kinematics.
'''
return forward_kinematics(*ik(pose), **dh_params)['flange']
'forward_kinematics': flanges,
'pentip_2d': penxyanoto,
'global_tool_orientation': penori
}
}
# save data to pickle
pickle_data = []
for k in xrange(len(flanges)):
tmp = {
'pen_pos': geometry_info['data']['pentip_2d'][k],
'pen_ori': geometry_info['data']['global_tool_orientation'][k][:3,:3],
'pen_fsr': numpy.NAN,
'pen_fsr_adc': numpy.NAN,
'robot_joints': ik(global_tars[k]),
'robot_tcp': forward_kinematics(*global_tars[0], **dh_params)['tcp'],
'robot_flange': forward_kinematics(*global_tars[0], **dh_params)['flange'],
}
pickle_data.append(tmp)
with open('calibration_verifitcation.pickle', 'wb+') as fp:
pickle.dump(pickle_data, fp)
with open('tool_wobj.yaml', 'w+') as fp:
json.dump({
'tool': {
'pos': list(dh_params['tool'][:3,3]),
'q': list(rot_to_quat(dh_params['tool'][:3,:3]))
},
'wobj': {
'pos': list(wobj[:3,3]),
def fk(pose):
return forward_kinematics(*pose, **dh_params)
def setUp(self):
self.dh_table = DH_TABLE
self.dh_table['tool'] = hom(0, 0, 0, [0.1, 0.12, 0.13])
self.joints = (0, 0, 0, 0, 0, 0)
self.tcp = forward_kinematics(*self.joints, **self.dh_table)['tcp']
def robot_frames(*joints, **dh_table):
return forward_kinematics(*joints, **dh_table)['robot_geometry_global']
def flange_from_joints(*joints, **dh_table):
return forward_kinematics(*joints, **dh_table)['flange']
def tcp_from_joints(*joints, **dh_table):
return forward_kinematics(*joints, **dh_table)['tcp']
def calc_robot_tcp(*joints):
return forward_kinematics(*joints, **dh_table)['tcp']
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'])
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):
print count
pl = StPlot()
robot_info = forward_kinematics(*path[count], **dh_table)
pl.draw_robot(robot_info['robot_geometry_global'])
pl.draw_trajectory(trajectory)
pl.draw_tool(robot_info['flange'],
dh_table['tool'])
pl.show()
pl.draw_joint_paths(path)
j3 = rand_range(-230, 50)
j4 = rand_range(-200, 200)
j5 = rand_range(-115, 115)
j6 = rand_range(-400, 400)
j1 = 10
j2 = 20
j3 = 30
j4 = 40
j5 = 50
j6 = 60
joint_values = j1, j2, j3, j4, j5, j6
# get forward kinematics i.e. last global robot-frame
robot_info = forward_kinematics(*joint_values, **DH_TABLE)
T44 = robot_info['tcp']
IK_angles = inverse_kinematics_irb140(DH_TABLE, T44)
# sanity check of forward kinematics
for angles in IK_angles.T:
info = forward_kinematics(*joint_values, **DH_TABLE)
t44 = info['tcp']
assert (norm(T44 - t44) < 1e-7)
# list of global-robot-frames
global_robot_frames = construct_robot_geometry(
info['robot_geometry_local'])
# generate a curve in the last global robot-frame
num_p = 50
def gen_robot_flanges(self):
robot_infos = map(lambda x: forward_kinematics(*x, **dh_table),
self.robot_joints)
flange = mat([x['flange'] for x in robot_infos])
flange[:, :3, 3] = flange[:, :3, 3] * 1000 #convert to mm
return flange