# Matlab Kinematics Toolbox

version: 0.1

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 , is identified by a skew symmetric matrix of the form:

The mapping from to is performed by the exponential formula and a closed-form solution exists through the Rodriguez formula. We refer to the matrix as a twist. Similar to Murray, we define the (vee) operator to extract the six-dimensional twist coordinates which parametrize a twist,

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

The adjoint operator provides a convenient method for transforming a twist from one coordinate frame to another. Given , the adjoint transform is a matrix which transforms twists from one coordinate frame to another.

the adjoint operator is invertible, and is given by:

## 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)

• 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
• 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

• 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

## References

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