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.

Peter Corke's - Robotics Toolbox for Matlab
Peter Kovesi's - Computer Vision functions for Matlab
Andrew Zisserman's - Multiple View Geometry functions for Matlab

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.

skew - 3x3 matrix, so(3)
skewcoords - 3x1 matrix which can be unpacked into a skew
twist - 4x4 matrix, se(3)
twistcoords - 6x1 matrix which can be unpacked into a twist
frame - coordinate frame described by a homogeneous transformation matrix, SE(3)
rotation - a 3x3 rotation matrix, SO(3)
traj - series of homogeneous transformations

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)

fast_skewexp - quickly calculate the exponential of a skew-symmetric matrix (good if needed in least-squares)
rotaxis - calculate the axis of rotation for a matrix R
rotparam - pulls a skew matrix and theta out of a rotation matrix
rotx - rotation matrix around the x-axis
roty - rotation matrix around the y-axis
rotz - rotation matrix around the z-axis
rpy - returns the X-Y-Z fixed angles of a rotation matrix
skew - generates a skew-symmetric matrix given a vector
skewcoords - generates a vector w given a skew-symmetric matrix
skewexp - calculate the exponential of a skew-symmetric matrix (similar to expm, but friendly to symbolic variables)
skewlog - calculate the log of a rotation matrix (similar to logm, but friendlier)

Operations in SE(3)

ad - Performs the adjoint transform
createtwist - Inputs a skew and a point, and returns a twist
fast_twistexp - quickly calculate the exponential of a twist matrix (good if needed in least-squares)
homdiff - compute differential between two homogeneous transforms in twist
homerror - calculates the error between two homogeneous transforms
homtotwist - finds a twist and a theta to create the homogeneous transform T
iad - performs the inverse adjoint transform
rotxh - homogeneous rotation matrix around the x-axis
rotyh - homogeneous rotation matrix around the y-axis
rotzh - homogeneous rotation matrix around the z-axis
twist - convert xi from a 6-vector to a 4 x 4 skew-symmetric matrix
twistaxis - inputs a twist and returns the axis
twistcoords - convert xi from a 4 x 4 skew-symmetric matrix to a 6-vector
twistexp - calculate the exponential of a twist matrix (similar to expm, but friendly to symbolic variables)
twistlog - calculate the log of a homogeneous matrix (similar to logm, but friendlier)
twistmagnitude - inputs a twist and returns the magnitude
twistpitch - inputs a twist and returns the pitch

Robot Links

bjacob - calculate the body jacobian for the robot
fkine - forward kinematics for serial link manipulator
ikine - inverse kinematics for serial link manipulator
ikine2 - iverse kinematics for serial link manipulator (alternate method)
robot - creates a robot structure
robotparams - returns a parameter vector from a robot (useful for numerically solving kinematic parameters)
sjacob - calculate the spatial jacobian for the robot

Logicals

ishom - returns true if the matrix is a homogeneous transform
isrobot - returns true if the structure is a robot
isrot - returns true if the matrix is a rotation matrix
isskew - returns true if the matrix is a skew-semmetric matrix
istwist - returns true if the matrix is a twist

Display

animframetraj - animates a series of frames
drawframe - plots a graphical description of a coordinate frame
drawframediff - plots the difference between frames
drawframetraj - plots the a series of homogeneous transforms
drawskew - plot a skew's axis of rotation
drawskewtraj - generates a graphical description of a screw over a series of thetas
drawtwist - plot a twist's axis of rotation
drawtwisttraj - generates a graphical description of a twist over a series of thetas

Helpers

pos - set or extract the translational part of a homogeneous matrix
rot - extracts the rotational part of a homogeneous matrix
transl - set or extract the translational part of a homogeneous matrix
noisehom - applies noise to a homogeneous transform
noiseskew - applies noise to a skew
noisetwist - applies noise to a twist
randhom - generates a random homogeneous transform
randskew - generates a random skew vector
randtwist - generates a random twist vector

Utility

arrow3 - plots an arrow
dimg - displays an image if global DebugLevel > level
dout - displays the string if global DebugLevel > level
dtor - converts degrees to radians
isequalf - returns true if the two quantities are equal within a threshold
man - emulates man function from the console
named_figure - selects a figure based on a string instead of by number
nice3d - make 3D plots look nicer
rtod - converts radians to degrees

Examples

example_robotlinks - an example using the ability to construct robots
example_swimmer - an example of the kinematics for our helical swimming robots (mpg)
test_screws - a test program I wrote for verifying the library (mpg)

References

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