Kinematics is the study of how things move.  In particular, we want to have methods for precisely calculating the position of points on a manipulator (static kinematics), and model how they will change as each joint moves (dynamic kinematics).  Further, we need to know which joint values will achieve a desired position (inverse kinematics) in order to produce controlled motions.

Manipulators are constructed from a series of links, each connected by an actuator.  A series of links is also called a "chain", with one end defined as the "base", and the other called the "end effector".  Actuators are commonly either "revolute" joints which cause rotation about an axis, or "prismatic" which cause translation along an axis.

On a robot such as the AIBO, each limb is a separate manipulator, defining a chain from a common base at the center of the robot to a different end effector (e.g. a paw or the camera).  All of the AIBO's joints are revolute.

Commonly, inverse kinematic algorithms solve only for the end effector position, where an industrial robot would have a gripper.  Since the AIBO has no grippers on most of its limbs, they are limited to non-prehensile manipulation.  To account for this, we have generalized the inverse kinematic solution so that you can solve for any point on (or off) of a link, and not just the bottom of the paw, which will allow you to do manipulation with any surface of the robot.

These diagrams are pulled from Sony's documentation for the models.  Numbers in italics were measured from the diagrams, whereas non-italic numbers were obtained from the model documentation.

At first glance, the reference frames may seem somewhat haphazard, but are determined by the Denavit-Hartenberg method of computing kinematic parameters.  The z axis of each link is determined by the axis of rotation for revolute joints, or the direction of extension for prismatic joints (which is used for the IR sensors and camera)  The x axis is then determined (sometimes with remaining freedom) by the relationship between consecutive links, and the y axis is then uniquely determined by the x and z axes.

For each model, the reference frame of the camera is made to concur with the reference frame used by the vision system, where positive x is to the AIBO's right, positive y is down, and positive z extends into the scene.

The shin link and related interest points for the ERS-7 were taken from photographic measurement since details of the diagram did not seem to match to actual production models.

When using the interest point diagrams, first find the letter of the point you would like to reference, then look at the corresponding name pattern shown in the list.  Most interest points are replicated on each of the limbs, so you will need to determine which limb you desire as well as whether you want the inner surface (closer to the body) or the outer surface (facing away from the body).

In the tables of distances in each diagram, italicized values were obtained from measurements, whereas the rest were obtained directly from documentation specifications.

The Kinematics class provides the recommended interface for kinematics computation.  This class integrates Tekkotsu's data structures with ROBOOP, which does the algorithmic heavy lifting (with the help of the newmat11 matrix library and some patches we've supplied as well).

There is a global instantiation of Kinematics, called kine, which always references the current values of WorldState.  Kinematics also serves as the base class for the PostureEngine class, so any time you have a PostureEngine or subclass thereof, you will be able to call the Kinematics functions on it to perform calculations regarding the pose it represents.

The types of calculations provided by Kinematics include forward kinematics (computing a point location from a set of joint values), ground plane estimation, and ray projection.  In addition, the PostureEngine class adds inverse kinematics (computing a set of joint values to reach a point location).

The ROBOOP package also provides some nice functions for computing motion dynamics, but unfortunately Sony has not provided specifications for the AIBO's motors in order to make use of this functionality.

Note that there are two reference frames associated with each effector - a static joint frame and a mobile link frame.  (Figure at right)  There is a link frame and a joint frame version of every function.  The difference is that the link frame of a given joint rotates with the joint, where as the joint frame is static.  Thus, points on the frame of the link are always at the same location in the link frame, but move in the joint frame as the effector moves.  The diagrams shown above illustrate the link frames for each joint.

The link configurations and interest points are stored in a file which is read by the robot when it boots up.  The main configuration file, config/tekkotsu.cfg, specifies the location of this file and the names of each chain which should be loaded.  By editing the kinematics configuration file for your model, you can add new reference frames and interest points if desired.

The kinematics configuration file is read by ROBOOP::Config, which doesn't provide any significant documentation of the format, although it is very straightforward.  Additional documentation regarding the specification of interest points and reference frames for use within Tekkotsu are at the end of the Kinematics class introduction.

Each end effector defines a chain of links from the base frame to that final reference frame.  Since ROBOOP does not support branching chains (which would be rather painful to implement) each chain must be specified from beginning to end, unfortunately resulting in duplicate specification of shared intermediary joints on related chains.  For instance, separate chains are needed for the camera and the mouth, and so the neck joints wind up being specified in both chains.

Implementation reference documentation is found in the Kinematics and PostureEngine classes, although you can also access the ROBOOP library directly, as is done by the look*() functions of HeadPointerMC.

Newmat's matrices are used for representing and manipulating information.  Points are represented with homogeneous column vectors, orientations are 3x3 matrices, and transformations are 4x4 matrices.

The ROBOOP library provides a number of functions in its utils.h for computing various transformational and rotational functions.  For instance, to find a transformation to rotate .2 radians around the x axis, you could call ROBOOP::rotx(.2).

The Kinematics class provides two convenience functions for putting coordinates in and out of NEWMAT::ColumnVectors, pack(x,y,z) and unpack(x,y,z).  Of course you can also create and manipulate ColumnVectors directly.  The Kinematics class then uses these Matrix and ColumnVector objects to provide functions for finding transformations between various links and reference frames.

Working with these matrices will require some basic knowledge of matrix algebra.  Although it's no substitute for a class on the subject, here's a quick refresher:

• A rotation matrix is a 3x3 matrix.  Each column is a unit vector, and can also be interpreted as the x, y and z vectors of the coordinate frame that will result after the rotation is applied.
• A transformation matrix is a 4x4 matrix.  The upper left 3x3 is the rotational component of the transformation, and the 4th column holds the translational offset.  The bottom row (in standard form) is [0 0 0 1].
• A homogeneous vector has four entries - the extra component holds the scaling of the vector.  Some examples:
  
• [1 2 3 1]' => (1, 2, 3)
• [1 2 3 2]' => (1/2, 2/2, 3/2) = (0.5, 1, 1.5)
• [1 2 3 .1]' => (1/.1, 2/.1, 3/.1) = (10, 20, 30)
• [1 2 3 0]' -> an infinite ray pointing along [1 2 3]
  
The last example illustrates why the homogeneous form is so useful - it allows us to preform useful computations regarding points at infinity.  It also allows us to apply both rotational and translational transformations using only a single matrix multiplication.

• To convert a point p using the transformation T, multiply the transformation by the point from the right, as in: T*p.
• Remember that matrix operations are not commutative.  In other words, A*B does not equal B*A.  (In fact, may not even be a legal operation unless they are both square.)
  
• To find the inverse of a rotation matrix, you can just take the transpose (t()).  However, to find the inverse of a full transformation matrix, you need to take the full inverse (i()).

We developed the calculations for inverse kinematics in MATLAB, and our simulation and visualization tools may be of use to others as well.  The newmat library was actually designed partly for MATLAB similarity, so it's fairly easy to port code between MATLAB and C++ - the calculations we have added to ROBOOP are almost a line-by-line copy of the MATLAB script.

In order to also test the final C++ code, we have begun work on allowing key Tekkotsu classes to compile on both Linux and Aperios (the AIBO's operating system).  This will allow you to move your code seamlessly from desktop to AIBO, but it has the drawback that we don't (yet) have any visualization in C++, where as MATLAB does.

All of these files are in the tools/test/kinematics directory.  Typing make will build the test_kinematics executable from the C++ sources.  Running MATLAB and typing test7 or test2xx will run a test script and show you a wireframe 3D model. (pictured at right)