R-R-R robot manipulator

Overview

The following program was an exercise in basic robot manipulator theory, as well as turning that theory into practice. This was one of my first programming attempts using OpenGL and SDL, and the robot code was written from scratch.

For the OpenGL/SDL part, I got started with some great tutorials from NeHe Productions.

The robot

The robot manipulator is set up as an R-R-R configuration (3 revolute joints) and moves on a 2D plane for the time being, even though the interface is in 3D. The manipulator setup and equations for describing the forward and inverse kinematics (IK) were inspired by this tutorial. The rotation and transformation matrices were set up with the help of the Eigen library for matrix manipulation. The forward kinematics solution is essentially straightforward, given that the user can control target tool position and orientation. A numerical method was used as a solution to the IK.

Forward kinematics

The parameters $\; \theta_i \;$ are the joint variables (angles of each joint $\; i \;$ ). The position $p$ and the angle of tool frame with respect to (w.r.t.) world frame (in degrees) $\; \phi \;$ are controlled by the user. For the forward kinematics and in order to draw each robot joint, joint coordinates need to be translated from one frame to the next in series. Rotation and translation matrices for moving from each joint $\; i \;$ to the next, along link length $\; a_i \;$ , are:

$R_i^{i+1} = \begin{bmatrix} \cos\theta & -\sin\theta & 0 & 0 \\ \sin\theta & \cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T_i^{i+1} = \begin{bmatrix} 1 & 0 & 0 & a_i \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

In order to end up with the representation of tool frame w.r.t. world frame, the rotation and translation matrices are multiplied in series, starting from the origin and moving towards the tool frame. The tricky part was to visualise the results in 3D using OpenGL commands. The robot links and joints were represented by a chain of appropriately oriented cylinders. I presented the rotation/transformation matrices as having three dimensions, as I hope to expand on this program and create an arm which can operate in 3D.

Inverse kinematics

The rotation matrix for rotation of tool frame w.r.t. world frame is:
$R_W^T = \begin{bmatrix} \cos\phi & -\sin\phi & 0 \\ \sin\phi & \cos\phi & 0 \\ 0 & 0 & 1 \end{bmatrix}$

The angle of the tool w.r.t. to the world frame can also be written as:
$\Theta = \theta_1 + \theta_2 + \theta_3 = atan2 \big( R_T^W(2,1), R_T^W(1,1) \big)$

Solving the IK problem comes down to a system of three non-linear equations:

$\theta_1+\theta_2+\theta_3 - \Theta = 0$
$a_1 cos \theta_1 + a_2 cos(\theta_1+\theta_2) + a_3 cos(\theta_1+\theta_2+\theta_3) - p_x = 0$
$a_1 sin \theta_1 + a_2 sin(\theta_1+\theta_2) + a_3 sin(\theta_1+\theta_2+\theta_3) - p_y = 0$

These equations can be solved by applying a gradient descent algorithm.

For a system of non-linear equations, we have:
$G(x) = \begin{matrix} [ y_1(\mathbf{x}) & y_2(\mathbf{x}) & y_3(\mathbf{x}) ]^T \end{matrix}$

$\mathbf{x} = \begin{matrix} [ x_1 & x_2 & x_3 ]^T \end{matrix}$

The objective function (to minimise):
$F(x) = \frac{1}{2} G^T G$

For each next guess $i+1$ :
$x^{(i+1)} = x^{(i)} - \gamma_i \nabla F(x^{(i)})$

, where $\nabla F(x^{(i)}) = J_G(x^{(i)})^T G(x^{(i)})$ (J is the Jacobian matrix).

The aim is to find a suitable $\; \gamma_0 \;$ so that $\; F(x^{(i+1)}) leq F(x^{(i)}) \;$ , until some threshold is reached.

In the robot manipulator’s case, $\; G \;$ is :
$G(\theta) = \begin{bmatrix} \theta_1 + \theta_2 + \theta_3 - atan2(r_{22},r_{11}) \\ \alpha_1 c_1 + \alpha_2 c_{12} + \alpha_3 c_{123} - p_x \\ \alpha_1 s_1 + \alpha_2 s_{12} + \alpha_3 s_{123} - p_y \end{bmatrix} = \begin{bmatrix} y_1(\mathbf{\theta}) \\ y_2(\mathbf{\theta}) \\ y_3(\mathbf{\theta}) \end{bmatrix}$
where:
$\mathbf{\theta} = \begin{matrix} [ \theta_1 & \theta_2 & \theta_3 ]^T \end{matrix}$ ,
$\; r_{22},r_{11} \;$ are the elements of the rotation matrix:
$R_3^0 = \begin{bmatrix} r_{11} & r_{12} & 0 \\ r_{21} & r_{22} & 0 \\ 0 & 0 & 1 \end{bmatrix}$ ,
and $\; c_i = cos(\theta_i), \; s_i = sin(\theta_i), \; c_{ij} = cos(\theta_i+\theta_j), \; s_{ij} = sin(\theta_i+\theta_j) \;$ , etc., for shorthand notation.

Jacobian:
$J = \begin{bmatrix} \frac{dy_1}{d\theta_1} & \frac{dy_1}{d\theta_2} & \frac{dy_1}{d\theta_3} \\ \frac{dy_2}{d\theta_1} & \frac{dy_2}{d\theta_2} & \frac{dy_2}{d\theta_3} \\ \frac{dy_3}{d\theta_1} & \frac{dy_3}{d\theta_2} & \frac{dy_3}{d\theta_3} \end{bmatrix} = \begin{bmatrix} 1 & 1 & 1 \\ -\alpha_1 s_1 - \alpha_2 s_{12} - \alpha_3 s_{123} & - \alpha_2 s_{12} - \alpha_3 s_{123} & -\alpha_3 s_{123} \\ \alpha_1 c_1 + \alpha_2 c_{12} + \alpha_3 c_{123} & \alpha_2 c_{12} + \alpha_3 c_{123} & \alpha_3 c_{123} \end{bmatrix}$

Images – Video

RRR robot GUI. The green square indicates the required target position that the manipulator end effector must achieve.

RRR robot GUI. The red square indicates that the target position is outside the arm’s reachable workspace.

Downloads

The program was compiled on Linux, Ubuntu 10.04 (kernel v2.6.32-26-generic). To compile, you will need to have these third-party libraries installed: OpenGL, SDL, Eigen v.2

Source: robotRRR.cpp

Makefile: Makefile

(Remove the .doc extensions, I had to add them to get the files uploaded on WordPress!)

Controls:

W,A,S,D – Move end-effector target on 2D plane

Q,E – Rotate end-effector target orientation left/right

Arrow keys – Rotate camera

Future goals

I hope to expand on this basic robot arm framework in many ways. Some initial tasks are:

Organise the code (multiple files, header files, perhaps set up an Eclipse project).
Expand the GUI’s functionality.
Add a revolute joint that will allow the arm to operate in 3D. This will be a more challenging coding project but will result in a much more useful arm!