Matlab Kinematics Toolbox

version: 0.1
author: Brad Kratochvil

About

The Matlab kinematics toolbox is something I put together over the course of my PhD to speed up prototyping robotics and computer vision related tasks at the Institute of Robotics and Intelligent Systems, ETH Zurich. Much of the mathematics (and indeed the inspiration for the library) comes from A Mathematical Introduction to Robotic Manipulation by Murray, Li, and Sastry.

Peter Corke's Robotics Toolbox has quite a bit of functionality in it, and I wasn't trying to reinvent the wheel. Much of this library was written as I was learning how to use twists for rigid-body computer vision applications as opposed to traditional robotics. My hope is that they are useful for others doing the same. The toolbox is broken up into functions that deal primarily with homogeneous transforms and their Lie algebra, and a set of functions for interacting with serial link kinematic structures. There are also quite a few functions for generating nice plots and animations of the results. I have included a few examples at the bottom of the page to get you started.

If you would like to download the entire library you can grab it from kinematics_toolbox.tgz, otherwise just browse the functions below. I primarily use Linux for a development environment, so the functions all work there. That being said, I've used the library in Windows (albeit not as much), and I'm guessing that much of it should work in Octave, but it's untested.

These functions are released under BSD licensing without any warranty. Please report any bugs and/or suggest enhancements to brad@kratochvil.name.

If you don't find what you need here, check out these other cool Matlab toolboxes.


Overview

The Special Euclidean group (3) is commonly know in the robotics literature as homogeneous transformations. The Lie algebra of SE(3), denoted $se(3)$, is identified by a $4\times4$ skew symmetric matrix of the form:

\begin{displaymath}
\left[\begin{array}{c c c c}
0 & -\omega_3 & \omega_2 & v_1...
... 0 & v_3 \\
0 & 0 & 0 & 0 \\
\end{array}\right] = \hat{\xi}
\end{displaymath}

The mapping from $se(3)$ to $se(3)$ is performed by the exponential formula $h = e^{\hat{\xi}}$ and a closed-form solution exists through the Rodriguez formula. We refer to the matrix $\hat{\xi}$ as a twist. Similar to Murray, we define the $\vee$ (vee) operator to extract the six-dimensional twist coordinates which parametrize a twist,

\begin{displaymath}
\left[\begin{array}{c c c c}
0 & -\omega_3 & \omega_2 & v_1...
... \omega_1 \\ \omega_2 \\ \omega_3 \\
\end{array}\right] = \xi
\end{displaymath}

The motion between consecutive frames can be represented by right multiplication of $h$ with a motion matrix $m$.

The adjoint operator provides a convenient method for transforming a twist from one coordinate frame to another. Given $m \in se(3)$, the adjoint transform is a $6 \times 6$ matrix which transforms twists from one coordinate frame to another.

\begin{displaymath}
m =
\left[\begin{array}{c c}
r & {\bf t} \\
{\bf0_{1 \times 3}} & 1 \\
\end{array}\right]
\end{displaymath}
\begin{displaymath}
ad(m) =
\left[\begin{array}{c c}
r & {\bf\hat{t}} r \\
{\bf0_{3 \times 3}} & r
\end{array}\right]
\end{displaymath}

the adjoint operator is invertible, and is given by:

\begin{displaymath}
ad^{-1}(m) =
\left[\begin{array}{c c}
r^t & -r^t \hat{\bf t} \\
{\bf0_{3 \times 3}} & r^t
\end{array}\right]
\end{displaymath}

Nomenclature

When browsing through the documentation, here are a few terms that might help you avoid confusion.

Also, Matlab has some built-in functions such as expm and logm that I've re-implemented for our skew-symmetric matrices. This is due to some occasional troubles that Matlab runs into finding solutions. The included functions (such as skewexp, skewlog, twistexp, and twistlog) can often be swapped directly for their Matlab counterparts.

Operations in SO(3)

skew.png

Operations in SE(3)

twist_1.png twist_0.png

Robot Links

Logicals

Display

trajectory.png

Helpers

Utility

Examples

swimmer.png example_movie.png

References

R. M. Murray and Z. Li and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press 1994.