-
Notifications
You must be signed in to change notification settings - Fork 78
/
IK_debug_numpy.py
438 lines (365 loc) · 16.5 KB
/
IK_debug_numpy.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
"""
Module for testing a numpy implementation of ROS node IK_server.py
Inverse kinematics is applied to convert a single end-effector position
in cartesian-space to its joint-space. Then Forward kinematics is applied
to recompute the end-effector's position to verify the correctness of the
joint-space.
"""
__author__ = 'Salman Hashmi, Sahil Juneja'
from time import time
from numpy import array, matrix, cos, sin, pi, arccos, arctan2, sqrt
from numpy.linalg import inv
import tf
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import pyplot as plt
# Test case format:
# -----------------
# [[[EE position],[EE orientation as quaternions]],
# [WC location],
# [joint angles]]
#
# To generate additional test cases:
# ----------------------------------
# Run '$ roslaunch kuka_arm forward_kinematics.launch', adjust the joint angles
# to find thetas. Use the gripper to extract positions and orientation
# (in quaternion xyzw) and use link 5 to find the position of the wrist center.
# These newly generated test cases can be added to the test_cases dictionary.
test_cases = {
1: [[[2.16135, -1.42635, 1.55109], [0.708611, 0.186356, -0.157931, 0.661967]],
[1.89451, -1.44302, 1.69366],
[-0.65, 0.45, -0.36, 0.95, 0.79, 0.49]],
2: [[[-0.56754, 0.93663, 3.0038], [0.62073, 0.48318, 0.38759, 0.480629]],
[-0.638, 0.64198, 2.9988],
[-0.79, -0.11, -2.33, 1.94, 1.14, -3.68]],
3: [[[-1.3863, 0.02074, 0.90986], [0.01735, -0.2179, 0.9025, 0.371016]],
[-1.1669, -0.17989, 0.85137],
[-2.99, -0.12, 0.94, 4.06, 1.29, -4.12]],
4: [],
5: []
}
# To store coordinates for plotting
received_ee_points = []
your_ee_points = []
ee_errors = []
def test_code(test_case):
"""
Calculate end-effector errors in computing Inverse kinematics.
Keyword arguments:
test_case -- a dictionary of test cases with following format
[[[EE position], [EE orientation quaternions]],
[WC location],
[joint angles]]
"""
x = 0
class Position:
"""Define position of end-effector."""
def __init__(self, EE_pos):
"""Initialize position object."""
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
"""Define orientation of end-effector."""
def __init__(self, EE_ori):
"""Initialize position object."""
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
"""Define combine object."""
def __init__(self, position, orientation):
"""Initialize combine object."""
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
"""Define end-effector pose."""
def __init__(self, comb):
"""Initialize end-effector pose object."""
self.poses = [comb]
req = Pose(comb)
start_time = time()
# INVERSE KINEMATICS ======================================================
def get_DH_Table():
"""
Define DH parameters for Kuka KR10 from its URDF file.
alphai-1 : angle b/w z-axes of links i-1 & i along x-axis of link i-1
ai-1 : dist b/w z-axes of links i-1 & i along x-axis of link i-1
di : dist b/w x-axes of links i-1 & i along z-axis of link i
thetai : angle b/w x-axes of links i-1 & i along z-axis of link i
"""
# Define variables for joint angles
theta1, theta2, theta3, theta4, theta5, theta6 = 0., 0., 0., 0., 0., 0.
# Construct DH Table with measurements from 'kr210.urdf.xacro' file
dh = {'alpha0': 0, 'a0': 0, 'd1': 0.75, 'theta1': theta1,
'alpha1': -pi/2, 'a1': 0.35, 'd2': 0, 'theta2': theta2,
'alpha2': 0, 'a2': 1.25, 'd3': 0, 'theta3': theta3,
'alpha3': -pi/2, 'a3': -0.054, 'd4': 1.50, 'theta4': theta4,
'alpha4': pi/2, 'a4': 0, 'd5': 0, 'theta5': theta5,
'alpha5': -pi/2, 'a5': 0, 'd6': 0, 'theta6': theta6,
'alpha6': 0, 'a6': 0, 'dG': 0.303, 'thetaG': 0}
return dh
def get_Rx(theta):
"""Define matrix for rotation (roll) about x axis."""
Rx = matrix([[1, 0, 0],
[0, cos(theta), -sin(theta)],
[0, sin(theta), cos(theta)]])
return Rx
def get_Ry(theta):
"""Define matrix for rotation (pitch) about y axis."""
Ry = matrix([[cos(theta), 0, sin(theta)],
[ 0, 1, 0],
[-sin(theta), 0, cos(theta)]])
return Ry
def get_Rz(theta):
"""Define matrix for rotation (yaw) about z axis."""
Rz = matrix([[cos(theta), -sin(theta), 0],
[sin(theta), cos(theta), 0],
[ 0, 0, 1]])
return Rz
def get_TF(alpha, a, d, theta):
"""Define matrix for homogeneous transforms between adjacent links."""
Tf = matrix([
[ cos(theta), -sin(theta), 0, a],
[sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d],
[ 0, 0, 0, 1]
])
return Tf
def get_ee_pose(pose_msg):
"""
Extract EE pose from received trajectory pose in an IK request message.
NOTE: Pose is position (cartesian coords) and orientation (euler angles)
Docs: https://github.com/ros/geometry/blob/indigo-devel/
tf/src/tf/transformations.py#L1089
"""
ee_x = pose_msg.position.x
ee_y = pose_msg.position.y
ee_z = pose_msg.position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[pose_msg.orientation.x, pose_msg.orientation.y,
pose_msg.orientation.z, pose_msg.orientation.w]
)
position = (ee_x, ee_y, ee_z)
print('******************************************************')
print('EE position: {}'.format(position))
orientation = (roll, pitch, yaw)
return position, orientation
def get_R_EE(ee_pose):
"""
Compute EE Rotation matrix w.r.t base frame.
Computed from EE orientation (roll, pitch, yaw) and describes the
orientation of each axis of EE w.r.t the base frame
"""
roll, pitch, yaw = ee_pose[1]
# Perform extrinsic (fixed-axis) sequence of rotations of EE about
# x, y, and z axes by roll, pitch, and yaw radians respectively
R_ee = get_Rz(yaw) * get_Ry(pitch) * get_Rx(roll)
# Align EE frames in URDF vs DH params through a sequence of
# intrinsic (body-fixed) rotations: 180 deg yaw and -90 deg pitch
Rerror = get_Rz(pi) * get_Ry(-pi/2)
# Account for this frame alignment error in EE pose
R_ee = R_ee * Rerror
return R_ee
def get_WC(dh, R_ee, ee_pose):
"""
Compute Wrist Center position (cartesian coords) w.r.t base frame.
Keyword arguments:
R_ee -- EE Rotation matrix w.r.t base frame
ee_pose -- tuple of cartesian coords and euler angles describing EE
Return values:
Wc -- vector of cartesian coords of WC
"""
ee_x, ee_y, ee_z = ee_pose[0]
# Define EE position as a vector
EE_P = matrix([[ee_x],
[ee_y],
[ee_z]])
# Get Col3 vector from R_ee that describes z-axis orientation of EE
Z_ee = R_ee[:, 2]
# WC is a displacement from EE equal to a translation along
# the EE z-axis of magnitude dG w.r.t base frame (Refer to DH Table)
Wc = EE_P - dh['dG']*Z_ee
return Wc
def get_joints1_2_3(dh, Wc):
"""
Calculate joint angles 1,2,3 using geometric IK method.
NOTE: Joints 1,2,3 control position of WC (joint 5)
"""
wcx, wcy, wcz = Wc[0], Wc[1], Wc[2]
# theta1 is calculated by viewing joint 1 and arm from top-down
theta1 = arctan2(wcy, wcx)
# theta2,3 are calculated using Cosine Law on a triangle with edges
# at joints 1,2 and WC viewed from side and
# forming angles A, B and C respectively
wcz_j2 = wcz - dh['d1'] # WC z-component from j2
wcx_j2 = sqrt(wcx**2 + wcy**2) - dh['a1'] # WC x-component from j2
side_a = round(sqrt((dh['d4'])**2 + (dh['a3'])**2), 7) # line segment: j3-WC
side_b = sqrt(wcx_j2**2 + wcz_j2**2) # line segment: j2-WC
side_c = dh['a2'] # link length: j2-j3
angleA = arccos((side_b**2 + side_c**2 - side_a**2) / (2*side_b*side_c))
angleB = arccos((side_a**2 + side_c**2 - side_b**2) / (2*side_a*side_c))
angleC = arccos((side_a**2 + side_b**2 - side_c**2) / (2*side_a*side_b))
# The sag between joint-3 and WC is due to a3 and its angle is formed
# between y3-axis and side_a
angle_sag = round(arctan2(abs(dh['a3']), dh['d4']), 7)
theta2 = pi/2 - angleA - arctan2(wcz_j2, wcx_j2)
theta3 = pi/2 - (angleB + angle_sag)
return theta1, theta2, theta3
def get_joints4_5_6(dh, R_ee, theta1, theta2, theta3):
"""
Calculate joint Euler angles 4,5,6 using analytical IK method.
NOTE: Joints 4,5,6 constitute the wrist and control WC orientation
"""
# Compute individual transforms between adjacent links
# T(i-1)_i = Rx(alpha(i-1)) * Dx(alpha(i-1)) * Rz(theta(i)) * Dz(d(i))
# account for 90 deg constant offset in theta2
T0_1 = get_TF(dh['alpha0'], dh['a0'], dh['d1'], theta1)
T1_2 = get_TF(dh['alpha1'], dh['a1'], dh['d2'], theta2-pi/2)
T2_3 = get_TF(dh['alpha2'], dh['a2'], dh['d3'], theta3)
# Extract rotation components of joints 1,2,3 from their
# respective individual link Transforms
R0_1 = T0_1[0:3, 0:3]
R1_2 = T1_2[0:3, 0:3]
R2_3 = T2_3[0:3, 0:3]
# Evaluate the composite rotation matrix formed by composing
# these individual rotation matrices
R0_3 = R0_1 * R1_2 * R2_3
# R3_6 is the composite rotation matrix formed from an extrinsic
# x-y-z (roll-pitch-yaw) rotation sequence that orients WC
# b/c R0_6 = R_ee = R0_3*R3_6
R3_6 = inv(array(R0_3, dtype='float')) * R_ee
r21 = R3_6[1, 0] # sin(theta5)*cos(theta6)
r22 = R3_6[1, 1] # -sin(theta6)*sin(theta6)
r13 = R3_6[0, 2] # -sin(theta5)*cos(theta4)
r23 = R3_6[1, 2] # cos(theta5)
r33 = R3_6[2, 2] # sin(theta4)*sin(theta5)
# Compute Euler angles theta 4,5,6 from R3_6 by individually
# isolating and explicitly solving each angle
theta4 = arctan2(r33, -r13)
theta5 = arctan2(sqrt(r13**2 + r33**2), r23)
theta6 = arctan2(-r22, r21)
return theta4, theta5, theta6
def handle_IK():
"""Simulates handle_calculate_IK()."""
dh = get_DH_Table()
# INVERSE KINEMATICS
ee_pose = get_ee_pose(req.poses[x])
R_ee = get_R_EE(ee_pose)
Wc = get_WC(dh, R_ee, ee_pose)
theta1, theta2, theta3 = get_joints1_2_3(dh, Wc)
theta4, theta5, theta6 = get_joints4_5_6(dh, R_ee, theta1, theta2, theta3)
return Wc, theta1, theta2, theta3, theta4, theta5, theta6
# FORWARD KINEMATICS ======================================================
Wc, theta1, theta2, theta3, theta4, theta5, theta6 = handle_IK()
dh = get_DH_Table()
dh['theta1'] = theta1
dh['theta2'] = theta2-pi/2 # account for 90 deg constant offset
dh['theta3'] = theta3
dh['theta4'] = theta4
dh['theta5'] = theta5
dh['theta6'] = theta6
# Compute individual transforms between adjacent links
# T(i-1)_i = Rx(alpha(i-1)) * Dx(alpha(i-1)) * Rz(theta(i)) * Dz(d(i))
T0_1 = get_TF(dh['alpha0'], dh['a0'], dh['d1'], dh['theta1'])
T1_2 = get_TF(dh['alpha1'], dh['a1'], dh['d2'], dh['theta2'])
T2_3 = get_TF(dh['alpha2'], dh['a2'], dh['d3'], dh['theta3'])
T3_4 = get_TF(dh['alpha3'], dh['a3'], dh['d4'], dh['theta4'])
T4_5 = get_TF(dh['alpha4'], dh['a4'], dh['d5'], dh['theta5'])
T5_6 = get_TF(dh['alpha5'], dh['a5'], dh['d6'], dh['theta6'])
T6_ee = get_TF(dh['alpha6'], dh['a6'], dh['dG'], dh['thetaG'])
# Create overall transform between base frame and EE by
# composing the individual link transforms
T0_ee = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_ee
your_wc = [Wc[0], Wc[1], Wc[2]] # Load computed Wc value
your_ee = [T0_ee[0, 3], T0_ee[1, 3], T0_ee[2, 3]] # Load computed Ee value
# ERROR ANALYSIS =========================================================
print("\nTotal run time to calculate joint angles from pose" +
" is %04.4f seconds" % (time()-start_time))
# Find WC error
if not(sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print("\n" +
"** These theta errors may not be a correct representation of your" +
"\ncode, due to the fact that the arm can have multiple positions." +
"\nIt is best to add your forward kinematics to confirm" +
"\nwhether your code is working or not.**")
print(" ")
# Find FK EE error
if not(sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
# Add EE points for plotting
print('')
received_ee_points.append(test_case[0][0])
your_ee_points.append(
[round(your_ee[0].item(0), 8),
round(your_ee[1].item(0), 8),
round(your_ee[2].item(0), 8)]
)
ee_errors.append(
[round(ee_x_e.item(0), 8),
round(ee_y_e.item(0), 8),
round(ee_z_e.item(0), 8)]
)
if __name__ == "__main__":
# Change test case number for different scenarios
for i in range(1, 4):
test_case_number = i
test_code(test_cases[test_case_number])
#print(received_ee_points)
#print(your_ee_points)
#print(ee_errors)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
data1 = array(received_ee_points)
x1, y1, z1 = data1.T
ax.scatter(x1, y1, z1, c='blue', s=50, marker='o')
ax.plot(x1, y1, z1, c='blue', label='rec_ee')
data2 = array(your_ee_points)
x2, y2, z2 = data2.T
ax.scatter(x2, y2, z2, c='orange', s=50, marker='s')
ax.plot(x2, y2, z2, c='orange', label='fk_ee')
data3 = array(ee_errors)
x3, y3, z3 = data3.T
ax.scatter(x3, y3, z3, c='magenta', s=50, marker='^')
ax.plot(x3, y3, z3, c='magenta', label='ee_error')
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
ax.set_zlabel('Z Axis')
ax.xaxis.label.set_color('red')
ax.yaxis.label.set_color('green')
ax.zaxis.label.set_color('blue')
ax.grid(True, which='both')
ax.view_init(30,220)
plt.legend()
plt.show()