RoboND-Kinematics-Project

Project: Kinematics Pick & Place

Rubric Points

Here I will consider the rubric points individually and describe how I addressed each point in my implementation.

1. Provide a Writeup / README that includes all the rubric points and how you addressed each one. You can submit your writeup as markdown or pdf.

You're reading it!

Kinematic Analysis

1. Run the forward_kinematics demo and evaluate the kr210.urdf.xacro file to perform kinematic analysis of Kuka KR210 robot and derive its DH parameters.

In this project, we are working with a simulation of Kuka KR210 to pick up cans from a shelf and then put them in a dropbox.

Note: For information on setting up and running this project, Link

Forward and Inverse Kinematics

Forward Kinematics (FK) is a set of methods to calculate the final coordinate position and rotation of end-effector of a conjoined links (e.g. robotic arms, limbs, etc.), given parameters of each joint between links. In this project, these parameters are angles of each joint, totalling 6 joints (i.e. 6 Degrees of Freedom).

Inverse Kinematics (IK), on the other hand, is the exact opposite of FK, where we calculate the parameters from a given coordinate position and rotation.

Homogenous Transforms

To calculate FK and IK calculation, we attach reference frames to each link of the manipulator and writing the homogeneous transforms from the fixed base link to link 1, link 1 to link 2, and so forth, all the way to the end-effector.

Denavit-Hartenberg (DH) Parameters

2. Using the DH parameter table you derived earlier, create individual transformation matrices about each joint. In addition, also generate a generalized homogeneous transform between base_link and gripper_link using only end-effector(gripper) pose.

To do FK and IK, we are using a method by Jacques Denavit and Richard Hartenberg which requires only four parameters for each reference frame.

Following are DH parameters used specifically in this project:

ID
1	0	0	0.75
2		0.35	0
3	0	1.25	0
4		-0.054	1.50
5		0	0
6		0	0
EE	0	0	0.303	0

Homogenous transforms are then combined together. Parameters of each transformation are set from DH parameters.Each transformation matrix looks like this:

!^{i-1}iT=\begin{bmatrix}cos(\theta_i) & - sin(\theta_i) & 0 & a \ sin(\theta_i)cos(\alpha{i-1}) & cos(\theta_i)cos(\alpha_{i-1}) & - sin(\alpha_{i-1}) & - d * sin(\alpha_{i-1}) \ sin(\theta_i)sin(\alpha_{i-1}) & cos(\theta_i)sin(\alpha_{i-1}) & cos(\alpha_{i-1}) & d * cos(\alpha_{i-1}) \ 0 & 0 & 0 & 1 \end{bmatrix}

[eq. 1]

Simplified as !^{0}_1T which means tranformation from reference frame A to reference frame B.

It is important to note (for later when we calculate to in this project) that this transformation matrix is consisted of rotational and translational matrices:

[eq. 2]

Or simplified as:

[eq. 3]

They should have come up with a better notation on this one, but !^{A}r_{P/A_0} means the position of point relative to , expressed in frame . It is confusing since here has a different meaning than in , in which the latter means element [1,1] of a rotational matrix.

!^{A}_BR denotes the rotational matrix from frame A to frame B. In other words, then, block from T matrix above looks as follows:

!^{i-1}iR=\begin{bmatrix}cos(\theta_i) & - sin(\theta_i) & 0 \ sin(\theta_i)cos(\alpha{i-1}) & cos(\theta_i)cos(\alpha_{i-1}) & - sin(\alpha_{i-1}) \ sin(\theta_i)sin(\alpha_{i-1}) & cos(\theta_i)sin(\alpha_{i-1}) & cos(\alpha_{i-1}) \end{bmatrix}

[eq. 4]

The links go from 0 to 6 and then followed by EE, that is why in the DH parameters above we have 7 rows. To combine transformations, calculate the dot products of all single transformations:

!^{0}{EE}T=^{0}{1}T * ^{1}{2}T * ^{2}{3}T * ^{3}{4}T * ^{4}{5}T * ^{5}{6}T * ^{6}{EE}T

[eq. 5]

3. Decouple Inverse Kinematics problem into Inverse Position Kinematics and inverse Orientation Kinematics; doing so derive the equations to calculate all individual joint angles.

Project Implementation

Basic Solution

From the diagram above, notice that reference frame 4, 5, and 6 intersect at the same coordinate. We treat frame 5 as the wrist center (WC), which then allow us to solve to analytically. to can then be found by solving

Joint 1 to 3

Here is a simplified diagram that shows frame 0 (ground) to frame 5 (or WC):

is pretty straightforward. We can get it by rotating about its Z-axis (). The diagram below shows the analytical method used to find and .

is the angle distance of and , thus from the diagram above we see that it can be calculated with the following formula:

[eq. 6]

Where was calculated from WC length and width relative to :

[eq. 7]

Note that here is different from in the DH parameters above.

can be calculated in a similar fashion:

[eq. 8]

And to calculate (epsilon):

[eq. 9]

Joint 4 to 6

From equation 5 above, using only the rotation parts from , and solve for reference frame 3 to 6, we have:

!^3_6R=inv(^0_3R) * ^0_6R

[eq. 10]

We then analyze the left and right hand sides of the equation above independently and solve for , , and .

Left-Hand-Side of the equation

!^3_6R can also be solved by calculating dot products of its components as described above in equation 5, or in other words, multiplying the matrix in equation 4 three times for , , and :

!^3_6R=^3_4R*^4_5R*^5_6R

!^3_6R=\begin{bmatrix} - s(\theta_4)c(\theta_6) + c(\theta_4)c(\theta_5)c(\theta_6) & - s(\theta_4)c(\theta_6) - s(\theta_6)c(\theta_4)c(\theta_5) & -s(\theta_5)c(\theta_4) \ s(\theta_5)c(\theta_6) & - s(\theta_5)s(\theta_6) & c(\theta_5) \ - s(\theta_4)c(\theta_5)c(\theta_6) - s(\theta_6)c(\theta_4) & s(\theta_4)s(\theta_6)c(\theta_5) - c(\theta_4)c(\theta_6) & s(\theta_4)s(\theta_5) \end{bmatrix}

[eq. 11]

Right-Hand-Side of the equation

!^0_6R can be found by evaluating the product of all rotations from to :

!^0_6R=R_{rpy}

[eq. 12]

(another r!), , and in this context stands for the roll, pitch, and yaw of the end-effector, which are known in this project.

Screenshoot Of Result and problems with basic solution

My IK computions takes too long and I was able to to get a 8/10 success rate as shown in the screeshot

IK Server Script:- IK_server.py Debug Script:- IK_debug.py

The result can be seen :

TODO Solution

Improved solution will be done after course is passed Need to lower the time taken for computation and see how to improve the result to get a success rate of 10/10