Here is the YouTube video showing the pick and plcace operation using the KR210 robotic arm
Following steps need to be followed to successfully find the IK solution of the robotic arm.
####Joints of KR210
Find the 3D coordinates of the six joints and the gripper using the kr210.urdf.xacro file. The XYZ reading are relative to the parent link so we take the cumulative sum of the readings.
To calculate the DH parameters of the KUKA kR210 the URDF file kr210.xacro.urdf was used.
| Link(i) | alpha( i-1 ) | a( i-1 ) | d( i-1 ) | theta( i ) |
|---|---|---|---|---|
| 1 | 0 | 0 | 0.75 | q1 |
| 2 | -pi/2 | 0.35 | 0 | q2 - pi/2 |
| 3 | 0 | 1.25 | 0 | q3 |
| 4 | -pi/2 | -0.054 | 1.5 | q4 |
| 5 | pi/2 | 0 | 0 | q5 |
| 6 | -pi/2 | 0 | 0 | q6 |
| G | 0 | 0 | 0.303 | 0 |
Storing these parameters as dictionary in python.
s = {
alpha0: 0 , a0: 0 , d1: 0.75,
alpha1: -pi/2. , a1: 0.35 , d2: 0. , q2: q2 - pi/2.,
alpha2: 0. , a2: 1.25 , d3: 0. ,
alpha3: -pi/2. , a3: -0.054, d4: 1.5,
alpha4: pi/2. , a4: 0. , d5: 0.,
alpha5: -pi/2. , a5: 0. , d6: 0.,
alpha6: 0. , a6: 0. , d7: 0.303 , q7: 0.
}To compose the matrix using the DH parameters for calculating the transform from frame 0 to frame i(for i = 1,2,3,4,5,6,G), let us first define transformation matrices for elementary operations.
Tx = Matrix([[ 1, 0, 0 , x_d ],
[ 0, cos(x_angle), -sin(x_angle), 0 ],
[ 0, sin(x_angle), cos(x_angle), 0 ],
[ 0, 0, 0, 1 ]
])
Ty = Matrix([[ cos(y_angle) , 0, sin(y_angle), 0 ],
[ 0, 1, 0, y_d ],
[ -sin(y_angle), 0, cos(y_angle), 0 ],
[ 0, 0, 0, 1 ]
])
Tz = Matrix([[ cos(z_angle) , -sin(z_angle) , 0, 0 ],
[ sin(z_angle), cos(z_angle) , 0, 0 ],
[ 0, 0 , 1, z_d ],
[ 0, 0 , 0, 1 ]
])
where x_angle, y_angle, z_angle are rotations about x, y and z axes and
x_d , y_d and z_d are translations along the x, y and z axes.
To find this homogeneous transform from frame (i-1) to frame (i) using the modified DH parameters, we apply the following 4 elementary operations:
- Rotation of
alpha(i - 1)about theX axis. - Displacement of
a(i - 1)along theX axis.
> Represented by `Tx.subs({x_angle: alpha, x_d: a})`
- Rotation of
theta(i)about theZ axis. - Displacement of
a(i)along theZ axis.
> Represented by `Tz.subs({z_angle: q, z_d: a})`
Thus, the application of the above operation gives the transformation matrix from frame (i-1) to frame (i).
Ti_minus_1_i = Tx.subs({x_angle: alpha, x_d: a}) * Tz.subs({z_angle: q, z_d: a})
By substituting the value of alpha, a, d and q, this transformation matrix will be used to find the individual transforms from :
frame 0toframe 1T0_1
frame 1toframe 2T1_2
frame 2toframe 3T2_3
frame 3toframe 4T3_4
frame 4toframe 5T4_5
frame 5toframe 6T5_6
frame 6togripper_frameT6_G
Transformation matrix from frame 0 to gripper_frame(corrected) will be obtained by the multiplication above transformation matrices.
Transformation matrix from frame 0 to frame 3 is T0_3 = T0_1 * T1_2 * T2_3.
T0_3 = [-sin(q3)*sin(q2 - 0.5*pi)*cos(q1) + cos(q1)*cos(q3)*cos(q2 - 0.5*pi), -sin(q3)*cos(q1)*cos(q2 - 0.5*pi) - sin(q2 - 0.5*pi)*cos(q1)*cos(q3), -sin(q1), -1.6*sin(q1) + 1.25*cos(q1)*cos(q2 - 0.5*pi) + 0.35*cos(q1)],
[-sin(q1)*sin(q3)*sin(q2 - 0.5*pi) + sin(q1)*cos(q3)*cos(q2 - 0.5*pi), -sin(q1)*sin(q3)*cos(q2 - 0.5*pi) - sin(q1)*sin(q2 - 0.5*pi)*cos(q3), cos(q1), 1.25*sin(q1)*cos(q2 - 0.5*pi) + 0.35*sin(q1) + 1.6*cos(q1)],
[ -sin(q3)*cos(q2 - 0.5*pi) - sin(q2 - 0.5*pi)*cos(q3), sin(q3)*sin(q2 - 0.5*pi) - cos(q3)*cos(q2 - 0.5*pi), 0, -1.25*sin(q2 - 0.5*pi)],
[ 0, 0, 0, 1]])This is done by applying 2 elementary operations:
- Rotation by
pi radiansthe about theZ axis
R_z = Tz.subs({z_angle: pi, z_d: 0})- Rotation by
-pi / 2about theY axis.
R_y = Ty.subs({y_angle: -pi/2, y_d: 0})Transformation matrix for correcting the gripper orientation wrt WC is:
R_corr = R_z * R_yInverse Positionis calculated by solving the jointsq1,q2andq3.Inverse Orientationis calculated using the jointsq4,q5andq6.
First calculated WC from EE coordinates using:
WC = EE - (0.303) * R_EE_num[:, 2]
where R_EE_num is the transform from base_frame to the End effector along with
- Corrected gripper orientation
- Substituted with correct Roll, Pitch and Yaw values.
0.303 is d(gripper).
theta1 = atan2( yc , xc)
where yx and xc are coordinates of the WC.
For calculating we look in the Z-X frame of reference.
For calculating theta2, because the side_a, side_b and side_c is known, cosine rule is used to find the angles of the triangle in the figure below.
theta2 = pi / 2 - angle_a - phiThe joint angle theta3 is also calculated using the angles found using the cosine rule.
theta3 = pi / 2 - offset_angle - angle_bangle_a , angle_b as the figure.
offset_angle is introduced because the centre points of j3 and j4, j5 and j6 are not collinear.
These joint angles can be calculated by observing the symbolic representation of R3_6 and manipulating its elements. It can out to be:
R3_6 = Matrix([
[-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)],
[ sin(q5)*cos(q6), -sin(q5)*sin(q6), cos(q5)],
[-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4), sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6), sin(q4)*sin(q5)]])
tan(theta4) = R3_6[2,2] / -R3_6[0,2]
tan(theta5) = sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2,2] * R3_6[2,2]) / R3_6[1,2])
tan(theta6) = -R3_6[1,1] / R3_6[1,0]
After finding out these angles, they were published to the RViz which moves the robotics arm KR210 also simulated in the Gazebo environment.
The inverse kinematics solution refers to finding the angles that the joints of the robotic arm need to be at so that the the end effector is at the desired position with the desired orientation. This can also be called the desired pose(position + orientation). This is a harder problem compared to forward kinematics (using joint angles to find the position and orientation of the end effector) and multiple solutions maybe present(in the dexterous workspace).
The inverse kinematics solutions were calculated in approximately 1 second per solution for the points in the path planned by RViz. For real time applications, this should be in the order of milliseconds.
This project can be improved by making a program that can produce closed-form inverse kinematics solutions automagically from the URDF file of the robot. Something like ik_fast. Shifting to compiled languages will also reduce time to calculate the IK solutions.
This will fail to calculate solutions if the point to be reached is at a greater distance than the workspace of the robot, although in real cases extenders might be used at the cost of payload capacity. Mobile manipulators is also another option for pick and placement of far away objects.