I have used the following environment to run this project:

• Ubuntu 16.04 LTS OS
• ROS Kinetic 1.12.12
• Gazebo 7.9
• RVIZ version 1.12.15 (Qt version 5.5.1 & OGRE version 1.9.0)

• Clone this repository to your home directory:

$ git clone https://github.com/mkhuthir/RoboND-Kinematics-Project.git ~/catkin_ws 

• As this project uses custom Gazebo 3D models, we need to add the path through environment variable:

$ echo "export GAZEBO_MODEL_PATH=~/catkin_ws/src/kuka_arm/models" >> ~/.bashrc

• Install missing ROS dependencies using the rosdep install command:

$ cd ~/catkin_ws/
$ rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y

• Run catkin_make from within your workspace to build the project:

$ cd ~/catkin_ws/
$ catkin_make

• Run the following shell commands to source the setup files:

$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc

• For demo mode make sure the demo flag is set to true in inverse_kinematics.launch file under ~/catkin_ws/src/kuka_arm/launch/

• You can also control the spawn location of the target object in the shelf by modifying the spawn_location argument in target_description.launch file under ~/catkin_ws/src/kuka_arm/launch/. 0-9 are valid values for spawn_location with 0 being random mode.

• To run forward kinematics test us:

$ roslaunch kuka_arm forward_kinematics.launch

• To run simulator use:

$ rosrun kuka_arm safe_spawner.sh

• To run IK Server use:

$ rosrun kuka_arm IK_server.py 

from the URDF file kr210.urdf.xacro we can extract the position xyz and orientation rpy of each joint from origin tag in each joint XML section:

for example, from the following fixed base joint XML section:

 <!-- joints -->
  <joint name="fixed_base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

In the origin tag:

<origin xyz="0 0 0" rpy="0 0 0"/>

We can see that xyz="0 0 0" and rpy="0 0 0".

Following table is showing complete extracted list for all joints base to gripper:

Using the above mentioned formulas, we can generate the DH parameters table as following:

in which q(i) is our input to joint angles (theta(i)).

I will be using python to code the forward kinematics:

To start with, we need the following imports:

import numpy as np
from numpy import array
from sympy import symbols, cos, sin, pi, simplify, sqrt, atan2, pprint
from sympy.matrices import Matrix

Python code to represent DH parameters table is:

# DH Table
DH_table = {alpha0:      0, a0:      0, d1:  0.75, q1:        q1,
     alpha1: -pi/2., a1:   0.35, d2:     0, q2: -pi/2.+q2,
     alpha2:      0, a2:   1.25, d3:     0, q3:        q3,
     alpha3: -pi/2., a3: -0.054, d4:  1.50, q4:        q4,
     alpha4:  pi/2., a4:      0, d5:     0, q5:        q5,
     alpha5: -pi/2., a5:      0, d6:     0, q6:        q6,
     alpha6:      0, a6:      0, d7: 0.303, q7:         0}

Python code for a function that will return the individual frame transformation matrix is as following:

# Function to return homogeneous transform matrix

def TF_Mat(alpha, a, d, q):
    TF = Matrix([[            cos(q),           -sin(q),           0,             a],
                 [ sin(q)*cos(alpha), cos(q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
                 [ sin(q)*sin(alpha), cos(q)*sin(alpha),  cos(alpha),  cos(alpha)*d],
                 [                 0,                 0,           0,             1]])
    return TF

Then using the following code to substitute the DH parameters into the transformation matrix:

   ## Substitute DH_Table
T0_1 = TF_Mat(alpha0, a0, d1, q1).subs(dh)
T1_2 = TF_Mat(alpha1, a1, d2, q2).subs(dh)
T2_3 = TF_Mat(alpha2, a2, d3, q3).subs(dh)
T3_4 = TF_Mat(alpha3, a3, d4, q4).subs(dh)
T4_5 = TF_Mat(alpha4, a4, d5, q5).subs(dh)
T5_6 = TF_Mat(alpha5, a5, d6, q6).subs(dh)
T6_7 = TF_Mat(alpha6, a6, d7, q7).subs(dh)

To get the composition of all transforms from base to gripper we simply multiply the individual matrices using the following code:

# Composition of Homogeneous Transforms
# Transform from Base link to end effector (Gripper)
T0_2 = (T0_1 * T1_2) ## (Base) Link_0 to Link_2
T0_3 = (T0_2 * T2_3) ## (Base) Link_0 to Link_3
T0_4 = (T0_3 * T3_4) ## (Base) Link_0 to Link_4
T0_5 = (T0_4 * T4_5) ## (Base) Link_0 to Link_5
T0_6 = (T0_5 * T5_6) ## (Base) Link_0 to Link_6
T0_7 = (T0_6 * T6_7) ## (Base) Link_0 to Link_7 (End Effector)

In order to apply correction needed to account for Orientation Difference Between definition of Gripper Link_7 in URDF versus DH Convention we need to rotate around y then around z axis:

R_y = Matrix([[ cos(-np.pi/2),           0, sin(-np.pi/2), 0],
              [             0,           1,             0, 0],
              [-sin(-np.pi/2),           0, cos(-np.pi/2), 0],
              [             0,           0,             0, 1]])

R_z = Matrix([[    cos(np.pi), -sin(np.pi),             0, 0],
              [    sin(np.pi),  cos(np.pi),             0, 0],
              [             0,           0,             1, 0],
              [             0,           0,             0, 1]])


R_corr = (R_z * R_y)

T_total= (T0_7 * R_corr)

Since the last three joints in KUKA KR210 robot (Joint_4, Joint_5, and Joint_6) are revolute and their joint axes intersect at a single point (Joint_5), we have a case of spherical wrist with joint_5 being the common intersection point; the wrist center (WC). This allows us to kinematically decouple the IK problem into Inverse Position and Inverse Orientation problems.

First step is to get the end-effector position(Px, Py, Pz) and orientation (Roll, Pitch, Yaw) from the test cases data class as shown in below code:

    # Requested end-effector (EE) position
    px = req.poses[x].position.x
    py = req.poses[x].position.y
    pz = req.poses[x].position.z
    
    # store EE position in a matrix
    EE = Matrix([[px],
                 [py],
                 [pz]])
    
    # Requested end-effector (EE) orientation
    (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(
        [req.poses[x].orientation.x,
         req.poses[x].orientation.y,
         req.poses[x].orientation.z,
         req.poses[x].orientation.w])

We will need rotation matrix for the end-effector:

R_rpy = Rot(Z, yaw) * Rot(Y, pitch) * Rot(X, roll)

and orientation difference correction matrix (Rot_corr) as earlier discussed in FK section.

R_EE = R_rpy * R_corr

We substitute the obtained roll, pitch and yaw in the final rotation matrix. Python Code is as following:

 # Find EE rotation matrix RPY (Roll, Pitch, Yaw)
    r,p,y = symbols('r p y')

    # Roll
    ROT_x = Matrix([[       1,       0,       0],
                    [       0,  cos(r), -sin(r)],
                    [       0,  sin(r),  cos(r)]])
    # Pitch
    ROT_y = Matrix([[  cos(p),       0,  sin(p)],
                    [       0,       1,       0],
                    [ -sin(p),       0,  cos(p)]])
    # Yaw
    ROT_z = Matrix([[  cos(y), -sin(y),       0],
                    [  sin(y),  cos(y),       0],
                    [       0,       0,       1]])

    ROT_EE = ROT_z * ROT_y * ROT_x

    # Correction Needed to Account for Orientation Difference Between
    # Definition of Gripper Link_G in URDF versus DH Convention

    ROT_corr = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
    
    ROT_EE = ROT_EE * ROT_corr
    ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})

The obtained matrix will be the rotation part of the full homogeneous transform matrix as yellow highlighted in the following:

    # Calculate Wrest Center
    WC = EE - (0.303) * ROT_EE[:,2]

WC is now having position of wrist center (Wx, Wy, Wz).

To find 𝜃1, we need to project Wz onto the ground plane Thus,

Theta1=atan2(Wy,Wx)

    # Calculate theat1
    theta1 = atan2(WC[1],WC[0])

Using trigonometry, we can calculate 𝜃2 and 𝜃3.

    #SSS triangle for theta2 and theta3
    A = 1.501
    C = 1.25
    B = sqrt(pow((sqrt(WC[0]*WC[0] + WC[1]*WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))

Now since we have all three sides of the triangle known to us we can calculate all of the three inner angles of the triangle from the known three sides Using trigonometry (specifically the Cosine Laws SSS type).

The same in Python code:

   a = acos((B*B + C*C - A*A) / (2*B*C))
   b = acos((A*A + C*C - B*B) / (2*A*C))
   c = acos((A*A + B*B - C*C) / (2*A*B))

Finally we calculate 𝜃2 and 𝜃3

    theta2 = pi/2 - a - atan2(WC[2]-0.75, sqrt(WC[0]*WC[0]+WC[1]*WC[1])-0.35)
    theta3 = pi/2 - (b+0.036) # 0.036 accounts for sag in link4 of -0.054m

For the Inverse Orientation problem, we need to find values of the final three joint variables 𝜃4, 𝜃5 and 𝜃6.

Using the individual DH transforms we can obtain the resultant transform and hence resultant rotation by:

R0_6 = R0_1R1_2R2_3R3_4R4_5*R5_6

Since the overall RPY (Roll Pitch Yaw) rotation between base_link and gripper_link must be equal to the product of individual rotations between respective links, following holds true:

R0_6 = R_EE

where,

R_EE = Homogeneous RPY rotation between base_link and gripper_link as calculated above.

We can substitute the values we calculated for 𝜃1, 𝜃2 and 𝜃3. in their respective individual rotation matrices and pre-multiply both sides of the above equation by inv(R0_3) which leads to:

R3_6 = inv(R0_3) * R_EE