2rp_kinematics_dh

Kinematics and Workspace of a 2-Link Planar Manipulator Using Python

Based on Denavit-Hartenberg (DH) Convention

This is the first in a series of articles which aims to layout procedure and let interactively develop kinematics of serial robots. We will also demonstarte how to carry out various useful analyses to understand the design and perfromance of robotic manipulators using Python programing language.

Table of Contents

Purpose

This post provides a basic workflow to derive and analyze kinematics of serial robotic manipulators using Python. Our goal is to provide a staright forward scheme to learn, derive and understand kinematics of serial robots in a nice and interactive manner.

For this purpose, we will be using Jupyter Notebook\Lab environmnt which provides a flexible way to interact with code and display inline plots. You may download and use this book as it is, or just grab the code and run it in any other Python development environment.

Prerequisites

Technical side: Know-how of robot kinemaics is recommened, but not necessary. For advanced usage, e.g. to extend this example to more complex designs, knwoeldge of DH (classic) notation is required. If you don't feel confident about that, don't worry! You should be able to learn this key concept in robotics through interactive playing here.

Programing side: Beginner to intermediate level skills with Python and Jupyter Notebooks would be handy. The python libraries we will use are:

  • Sympy (for symbolic computation)
  • Numpy (for numerical computaion)
  • Plotly (for 3D interactive plotting)

Note: Assuming that you already know how to setup Python and Jupyter Notebook environment, it is skipped here.

Environment Setup

First, we import the SymPy libraray that will allow us to develop and manipulate symbolic expressions.

In [1]:
import sympy as sp

In SymPy, we initialize printing so that all of the mathematical expressions can be rendered in standard mathematical notation.

In [2]:
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)

The Jupyter notebook can use IPython to display rich content. Here, We will use the Image function to import the geometric represnttion of 2-link manipulator example.

In [3]:
from IPython.display import Image
Image('fig/2rp_new.png', width=300)
Out[3]:

Why 2RP Manipulator?

Almost every (under)gradute level robotics textbook, as far as I know, starts teaching robot kinematics using a simple and basic manipulator example called 2-link planar manipulator (sometimes called as 2RP manipulator, R stands for the revolute joints). For sake of simplicity and as a starting point, we also start with the 2-link planar manipulator example which should be easy to follow and understad the underlying science.

Now, we declare the symbols (link lenghts, joint variables etc.) which will be used for further formulation.

In [4]:
from sympy.physics.mechanics import dynamicsymbols
In [5]:
theta1, theta2, l1, l2, theta, alpha, a, d = dynamicsymbols('theta1 theta2 l1 l2 theta alpha a d')
theta1, theta2, l1, l2, theta, alpha, a, d
Out[5]:
$$\left ( \theta_{1}, \quad \theta_{2}, \quad l_{1}, \quad l_{2}, \quad \theta, \quad \alpha, \quad a, \quad d\right )$$

Kinematics Using DH Method

We assign the frames to our manipulator according to DH classic convention. DH method provides a convenient way to express the position and orienation of a rigid-body in matrix forms using the relevant link lengths, joint angles, twist angles and joint offsets.

DH Table

The DH table for 2-Link Planar Mnipulator shown above can be formulated as,

\begin{array}{cccc} \hline \hline i & \alpha_{i} & a_{i} & d_i & \theta_i \\ \hline \hline 1 & 0 & l_1 & 0 & \theta_1 \\ 2 & 0 & l_2 & 0 & \theta_2 \\ %3 & -\frac{\pi}{2} & 0 & 0 & -\frac{\pi}{2} - \phi_1 \\ %4 & 0 & 0 & l_1 & 0 \\ \hline \end{array}

$i$: joint number, $\alpha_i$: twist angle, $a_i$: length of link $i$, $d_i$: joint offset and $\theta_i$: joint angle.

Homogenous Transformation

The standard homogenous transformation matrix (transaformation from base to tip frame) is represnted as,

In [6]:
rot = sp.Matrix([[sp.cos(theta), -sp.sin(theta)*sp.cos(alpha), sp.sin(theta)*sp.sin(alpha)],
                 [sp.sin(theta), sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha)],
                 [0, sp.sin(alpha), sp.cos(alpha)]])

trans = sp.Matrix([a*sp.cos(theta),a*sp.sin(theta),d])

last_row = sp.Matrix([[0, 0, 0, 1]])

m = sp.Matrix.vstack(sp.Matrix.hstack(rot, trans), last_row)
m
Out[6]:
$$\left[\begin{matrix}\operatorname{cos}\left(\theta\right) & - \operatorname{sin}\left(\theta\right) \operatorname{cos}\left(\alpha\right) & \operatorname{sin}\left(\alpha\right) \operatorname{sin}\left(\theta\right) & a \operatorname{cos}\left(\theta\right)\\\operatorname{sin}\left(\theta\right) & \operatorname{cos}\left(\alpha\right) \operatorname{cos}\left(\theta\right) & - \operatorname{sin}\left(\alpha\right) \operatorname{cos}\left(\theta\right) & a \operatorname{sin}\left(\theta\right)\\0 & \operatorname{sin}\left(\alpha\right) & \operatorname{cos}\left(\alpha\right) & d\\0 & 0 & 0 & 1\end{matrix}\right]$$

Transformation: frame '0' to '1'

In [7]:
m01 = m.subs({alpha:0, a:l1, theta:theta1, d:0})
m01
Out[7]:
$$\left[\begin{matrix}\operatorname{cos}\left(\theta_{1}\right) & - \operatorname{sin}\left(\theta_{1}\right) & 0 & l_{1} \operatorname{cos}\left(\theta_{1}\right)\\\operatorname{sin}\left(\theta_{1}\right) & \operatorname{cos}\left(\theta_{1}\right) & 0 & l_{1} \operatorname{sin}\left(\theta_{1}\right)\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$$

Transformation: frame '1' to '2'

In [8]:
m12 = m.subs({alpha:0, a:l2, theta:theta2, d:0})
m12
Out[8]:
$$\left[\begin{matrix}\operatorname{cos}\left(\theta_{2}\right) & - \operatorname{sin}\left(\theta_{2}\right) & 0 & l_{2} \operatorname{cos}\left(\theta_{2}\right)\\\operatorname{sin}\left(\theta_{2}\right) & \operatorname{cos}\left(\theta_{2}\right) & 0 & l_{2} \operatorname{sin}\left(\theta_{2}\right)\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$$

Homogenous transformation: frame '0' to '2'

In [9]:
m02 = (m01*m12)
m02
Out[9]:
$$\left[\begin{matrix}- \operatorname{sin}\left(\theta_{1}\right) \operatorname{sin}\left(\theta_{2}\right) + \operatorname{cos}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) & - \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) - \operatorname{sin}\left(\theta_{2}\right) \operatorname{cos}\left(\theta_{1}\right) & 0 & l_{1} \operatorname{cos}\left(\theta_{1}\right) - l_{2} \operatorname{sin}\left(\theta_{1}\right) \operatorname{sin}\left(\theta_{2}\right) + l_{2} \operatorname{cos}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right)\\\operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) + \operatorname{sin}\left(\theta_{2}\right) \operatorname{cos}\left(\theta_{1}\right) & - \operatorname{sin}\left(\theta_{1}\right) \operatorname{sin}\left(\theta_{2}\right) + \operatorname{cos}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) & 0 & l_{1} \operatorname{sin}\left(\theta_{1}\right) + l_{2} \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) + l_{2} \operatorname{sin}\left(\theta_{2}\right) \operatorname{cos}\left(\theta_{1}\right)\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$$

Using Sympy's built-in simplification methods, we simplify the transformation matrix as

In [10]:
mbee= sp.Matrix([[m02[0,0].simplify(), m02[0,1].simplify(), sp.trigsimp(m02[0,3].simplify())],
                 [m02[1,0].simplify(), m02[1,1].simplify(), sp.trigsimp(m02[1,3].simplify())],
                 [m02[2,0].simplify(), m02[2,1].simplify(), m02[2,2].simplify()]])

mbee
Out[10]:
$$\left[\begin{matrix}\operatorname{cos}\left(\theta_{1} + \theta_{2}\right) & - \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) & l_{1} \operatorname{cos}\left(\theta_{1}\right) + l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\\\operatorname{sin}\left(\theta_{1} + \theta_{2}\right) & \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) & l_{1} \operatorname{sin}\left(\theta_{1}\right) + l_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\\0 & 0 & 1\end{matrix}\right]$$

The first two columns in above matrix describe the orienation of the tip of manipulator in X, Y and Z directions. Whereas, the last column represnts the position in respective directions. As our manipulator does not carry any end-effector, analyses of orientaion becomes meaningless.

Forward Kinematic Equations

Tip position can be expressed as,

Position in x-direction

In [11]:
px = mbee[0,2]
px
Out[11]:
$$l_{1} \operatorname{cos}\left(\theta_{1}\right) + l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)$$

Position in y-direction

In [12]:
py = mbee[1,2]
py
Out[12]:
$$l_{1} \operatorname{sin}\left(\theta_{1}\right) + l_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)$$

Evaluation of tip position

To evaluate tip position numerically, we make use of Sympy's lambdify function which takes $l_1$, $l_2$, $\theta_1$ and $\phi_1$ as arguments.

In [13]:
fx = sp.lambdify((l1, l2, theta1, theta2), px, 'numpy')
fy = sp.lambdify((l1, l2, theta1, theta2), py, 'numpy')

To plot tip position in X and Y, we import Matplotlib and Numpy. For inline plot display, we use Ipython magic.

In [14]:
import matplotlib.pyplot as plt
%matplotlib inline
import numpy as np
d2r = np.deg2rad
In [15]:
theta1s = np.linspace(d2r(0), d2r(360)) # desired range of motion for joint 1
theta2s = np.linspace(d2r(0), d2r(360)) # desired range of motion for joint 2

zx = np.array(fx(15.0, 15.0, theta1s, theta2s))
zy = np.array(fy(15.0, 15.0, theta1s, theta2s))

fig, ax1 = plt.subplots()
ax1.plot(np.rad2deg(theta1s), zx, label = r'$p_x$')
ax1.plot(np.rad2deg(theta1s), zy, label = r'$p_y$')
ax1.set_xlabel(r'($\theta_1$, $\theta_2$) [deg]')
ax1.set_ylabel(r' tip position [mm]')
plt.legend()
plt.grid()

Manipulator Workspace

The next step is to visualize the workspce of our manipulator. For this purpose, we will import Plotly and make sure that it displays inline plot in our notebook environment.

In [16]:
from plotly.offline import download_plotlyjs, init_notebook_mode, plot, iplot
init_notebook_mode(connected=True) # for offline mode in Jupyter Notebook use

import plotly.offline as py
import plotly.graph_objs as go
from numpy import * # Not recommended, but we use to avoid rewriting the forward kinematic equations with prefix 'np'
In [17]:
theta11 = np.linspace(d2r(0),d2r(90))
theta22 = np.linspace(d2r(0), d2r(360))
theta1, theta2 = np.meshgrid(theta11, theta22)
l_range = [5] # we can use more than one value here

px1 = {}
py1 = {}
pz1 = {}
for i in l_range:
    l1 = i
    l2 = i - 4
    
    pxa = l1*cos(theta1) + l2*cos(theta1 + theta2)
    pya = l1*sin(theta1) + l2*sin(theta1 + theta2)
    
    px1['x{0}'.format(i)] = pxa
    py1['x{0}'.format(i)] = pya
In [18]:
pxx = px1['x5']
pyy = py1['x5']
pzz = pyy*0 #dummy zero points for z-axis, as it doesn't exist
In [19]:
trace1 = go.Surface(z=pzz, x=pyy, y=pxx,
                    colorscale='Reds', 
                    showscale=False, 
                    opacity=0.7,
                   )
data = [trace1]
In [20]:
layout = go.Layout(scene = dict(
                    xaxis = dict(title='X (mm)'),
                    yaxis = dict(title='Y (mm)'),
                    zaxis = dict(title='Z (mm)'),
                    ),
                  )
In [21]:
fig = go.Figure(data=data, layout=layout)
py.iplot(fig)

Use mouse to explore the workspace plot.         Wheel: Zoom in/out, Left: Rotate, Right: Pan

This concludes our post. To summarize, we analytically derived the kinematics of 2-link manipulator using DH notation and visualized its tip position and workspace.

In the next post, we will learn how to derive the same kinematic equations without using DH parameters. We will use Sympy to compute the kinematics analytically and will see if the obtained results are identical to the ones presented in this post.