2rp_kinematics_sympy

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

Based on Sympy (Symbolic Python) Mechanics

This is the second post of the series on robot kinematocs using Python programming languge. In the previous post, we had shown how to develope the kinematics and visulaize workspace of a 2-link planar manipulator. For that purpose, we had used the DH method to assign joint frames and then computed the transformation matrices accordingly.

In this post, we will achieve the same objectives but withput using DH parameters. Seems pretty interesting... !! So, let's dive in.

Table of Contents

Prerequisites

Just to remind, 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.

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.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, Point, ReferenceFrame
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 Sympy Mechanics

In first step, we define the kinematic relationships among rigid bodies of the 2-link manipulator shown above. We will make use of Sympy's ReferenceFrame objects to describe the four frames, define their orientations, and then construct vectors in the frames that position various important Points. Our objective is to analytically calculate the tip position in XY (Cartesian) space.

Reference Frames Assigment

To start with, we define three reference frames, one each for the base, shoulder joint, elbow joint and the hand. These reference frames hold the information that defines how each frame is translted and/or oriented relative to each other. We start by creating four reference frames for each frame in the system. Here, base frame acts as the inertial frame of reference.

In [6]:
base_frame = ReferenceFrame('B')
shoulder_frame = ReferenceFrame('S')
elbow_frame = ReferenceFrame('E')
hand_frame = ReferenceFrame('H')

Frame Orientation Definition

Now we need to specify how the frames are oriented with respect to each other. To do this, we define the two generalized coordinates, $\theta_1(t)$ and $\theta_2(t)$, for the shoulder joint and elbow joint angles which are time varying quantities. As these joints are held at distance by rigid links, we use $l_1$ and $l_2$ to repesent the distance each frame from its previous successive farme.

We start by setting the orientation of the base frame as the inertial reference frame. Then the shoulder frame orients equal to $theta_1$ along the z-axis of base frame. And, we keep on defining the next frames orientation according to the physical mechanics of the 2-link manipulator.

In [7]:
shoulder_frame.orient(base_frame, 'Axis', [theta1, base_frame.z])
elbow_frame.orient(shoulder_frame, 'Axis', [theta2, shoulder_frame.z])
hand_frame.orient(elbow_frame, 'Axis', [0, elbow_frame.z])

Forward Kinematics

Homogeneous Transformation

To obtain the transformation matrix from base frame to hand frame, we use Sympy's direction cosine matrix method.

In [8]:
T02 = base_frame.dcm(hand_frame)
T02.simplify()
Out[8]:
$$\left[\begin{matrix}\operatorname{cos}\left(\theta_{1} + \theta_{2}\right) & - \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) & 0\\\operatorname{sin}\left(\theta_{1} + \theta_{2}\right) & \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) & 0\\0 & 0 & 1\end{matrix}\right]$$

Rigid Body Points and Locations

To derive the forward kinematic equations, we need to defin the the length of each rigid body. We will first define points that locate the joints: base, shoulder, elbow and hand.

In [9]:
base = Point('O')
shoulder = Point('0')
elbow = Point('1')
hand = Point('2')

Nextm we set the position with the Point.set_pos() method, giving the reference point and the vector.

In [10]:
shoulder.set_pos(base, 0 * base_frame.x)
elbow.set_pos(shoulder, l1 * shoulder_frame.x)
hand.set_pos(elbow, l2 * elbow_frame.x)

Base to Hand Position

In [11]:
pxy = (hand.pos_from(base).express(base_frame)).simplify()
pxy
Out[11]:
$$(l_{1} \operatorname{cos}\left(\theta_{1}\right) + l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right))\mathbf{\hat{b}_x} + (l_{1} \operatorname{sin}\left(\theta_{1}\right) + l_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right))\mathbf{\hat{b}_y}$$

Position in x-direction

In [12]:
px = l1*sp.cos(theta1) + l2*sp.cos(theta1 + theta2)
px
Out[12]:
$$l_{1} \operatorname{cos}\left(\theta_{1}\right) + l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)$$

Position in y-direction

In [13]:
py = l1*sp.sin(theta1) + l2*sp.sin(theta1 + theta2)
py
Out[13]:
$$l_{1} \operatorname{sin}\left(\theta_{1}\right) + l_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)$$

Tip Position Evaluation

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 [14]:
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 [15]:
import matplotlib.pyplot as plt
%matplotlib inline
import numpy as np
d2r = np.deg2rad
In [16]:
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 [17]:
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 [18]:
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 [19]:
pxx = px1['x5']
pyy = py1['x5']
pzz = pyy*0 #dummy zero points for z-axis, as it doesn't exist
In [20]:
trace1 = go.Surface(z=pzz, x=pyy, y=pxx,
                    colorscale='Reds', 
                    showscale=False, 
                    opacity=0.7,
                   )
data = [trace1]
In [21]:
layout = go.Layout(scene = dict(
                    xaxis = dict(title='X (mm)'),
                    yaxis = dict(title='Y (mm)'),
                    zaxis = dict(title='Z (mm)'),
                    ),
                  )
In [22]:
fig = go.Figure(data=data, layout=layout)
py.iplot(fig)

Use mouse to exlore the 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 Sympy module and visualized its tip position and workspace.If we compare the obtained reults (kinematic equations, workspace etc.), they're perfectly identical to the ones obtained through DH method.

Next!

In the next post, we will learn how to carry out joint to tip velocity mappings. We will see how to calculate the Jacobean matrix analytically and find out singular configurations of the 2-link manipulator.