Robotic Arm Pick and Place
This page will describe how to do forward and inverse kinematic analysis to control the end point effector of a robotic pick and place arm using the robot operating system (ROS). Below is a walkthrough of the fundamental principles and methods used in the project and it's subsequent Python implementation. The information supplied is not exhaustive, and it is recommended to view the source code and comments on GitHub for a deeper understanding of the project. A link to the GitHub repository is here. You can also see a video of the project in action here.
This project page will describe the following sections
Forward Kinematics
The robotic arm used in the project is the Kuka KR210, a six degree of freedom (6DOF) revolute joint or an RRRRRR robot. Below is a diagram of the arms links and offset used to derive the DenavitHartenberg (DH) parameters.
The
Now that we have the transforms and kinematics calculations for the angles, we can code our robot arm. To view the code implementation of this maths, check out the IK_server.py file in the project repository available from GitHub here.
A video of the KR210 robot arm in Gazebo and RViz can be viewed below.
In 1955, Jacques Denavit and Richard Hartenberg proposed a systematic method of attaching reference frames to the links of a manipulator that simplified the homogeneous transforms from six independent parameters per joint to only four to describe the position and orientation of adjacent reference frames. Since its original description, several modifications have been made to the DH method. The method used in this analysis will be the original method, as described in M Spong, S Heutchinson & M Vidyasagar. (2006). Robot Modelling and Control, (Wiley & Sons, Inc., NJ). The DH parameters involved are α, a, d, and θ. The image below describes how these parameters are used and linked with an RRPR robotic arm.
The joints are assigned as follows;

Joint i is fixed with respect to link i – 1, and so is set in reference frame i – 1.

Joint 1 is set in reference frame 0, which is the base frame.

Joint variable qi represents either θ for a rotation (revolute) or displacement (d) for a prismatic joint.

DH matrix Ai = Ai (qi ) is the transformation matrix from frame i – 1 to i, derived from joint and link i.
The coordinate frames are assigned as follows;
1. Zi1 is the axis of actuation of joint i.
– Axis of revolution of revolute joint
– Axis of translation of prismatic joint
2. Axis Xi is set so it is perpendicular to and intersects Zi1.
3. Derive Yi from Xi and Zi using the righthand sense.
Once the coordinate frame has been set, the parameters involved are α, a, d, and θ are derived as follows;

ai is the distance from Zi1 to Zi measured along Xi.

αi is the twist angle from Zi1 to Zi measured about Xi in a righthand sense.

di is the distance from Xi1 to Xi measured along Zi1. Note that this quantity will be a variable in the case of prismatic joints.

θi (joint angle) is the angle from Xi−1 to Xi measured about Zi1 in a righthand sense. Note that this quantity will be a variable in the case of a revolute joint.
We can now derive the parameters for the Kuka KR210 arm displayed above. The diagram below shows the arm in a zero state with a table of the DH parameters.
Now that we have the DH parameters, we can now substitute the values into the homogeneous transformation matrix for each joint. The homogeneous transformation matrix is
Substituting the modified DH parameters for each link into the homogeneous transformation matrix and simplifying, we obtain the following link matrices. For clarity of the matrices, Cθ1 will be represented as C1.
Link 1: a = a1, α = π /2 , d = d1, θ = θ1
Inverse Kinematics
Forward kinematics is good to calculate the grippers location if we know the joint angles θ. However, with a pick and place robot arm, we only know the position of the object we require to pick up. We could just guess the joint angles and uses forward kinematics to see if the angles place the gripper in the correct location but with a large number of angle combinations for a 6DOF robot, it is not a feasible option. A better option is to work from the gripper location and calculate the angles required to have the gripper in the desired position. This is called inverse kinematics. Unfortunately, there is no elegant solution similar to the forward kinematics methods, but this also means there are multiple ways to calculate the inverse kinematics. The method outlined here we will use trigonometry to calculate each angle from the desired gripper position.
To determine the joint angles, the KR210 arm can be divided into two sections. The base arm section containing joints 1, 2 and 3 and the wrist containing joints 4, 5 and 6. The coordinate frames of the robot are fixed at the base of the robot with the Xaxis, the yaxis is perpendicular the robot arms and the zaxis is vertical, towards the sky. The following image shows the world coordinates with the arm in a zero angle state. The xaxis is red, the yaxis is green and the zaxis is blue.
The arm calculation of joints 2 and 3 can be derived by using the law of cosines. This law is outlined below. For the arm calculations, we can say that point A is joint 2, coordinate frame x1, y1, z1,
The remaining joints make up the wrist section of the arm, joints 4, 5, & 6, and joint angles have been derived using trigonometric laws and the calculated angles from joints 1, 2 and 3.
Joint 4 is the tangent of a circle where its centre point is the grippers x, y, z points when all angles are at zero. I.e., joint 4 will rotate around the centre point as if it was the tangent of the circle with a zero angle when at the bottom, pi/2 rotation angle on the left side, pi rotation when verticle and 3*pi/2 on the right side. Joint 5 is the angle that creates the most direct line to the target thus pointing the gripper towards the target. Joint 6 is the negative angle of Joint 4.
Below are the calculations of all six joint angles.
As the grippers desired position is relative to the worlds base frame above, joint 1 is just the tan angle of rotation from the world xaxis to the grippers x, y coordinates.
Link 2: a = a2, α = 0, d = 0, θ = θ2 + π/2
Link 3: a = a3, α = π/2, d = 0, θ = θ3
Link 4: a = 0, α = π/2, d = d4, θ = θ4
Link 5: a = 0, α = π/2, d = 0, θ = θ5
Link 6: a = 0, α = 0, d = d6, θ = θ6
If you wish to calculate the transform between the base, link 1, and the gripper, link 6, all that is required is to multiply the transformation matrices together. We will not derive that here due to the complexity and size of the transform but can instead use python and the python library Sympy.
For our use case with the KR210, we can say that point A is joint 2, coordinate frame x1, y1, z1, point C is joint 3, coordinate frame x2, y2, z2, and point B is the centre of the wrist, coordinate frame x4, y4, z4. Length b is link 2's length a2 and length a is the Pythagorean of lengths d4 and a3. The endpoint of point B is the desired objects picking location minus the length of d6 along the aaxis of the world reference frame. Let us start with length a which we will call R3.
XB = Xp  d6 Where (Xp, Yp, Zp) is the target objects position.
YB = Yp
ZB = Zp
X0 = XB  a1 * cosθ1 Where (X0, Y0, Z0) is the origin of the coordinate frame of joint 2.
Y0 = YB  a1 * sinθ1
Z0 = ZB  d1
Z0 = d1 + a2
TY = Yp
TZ = Zp  Z0