Kinematics

Forward Kinematics

Given a configuration of the robot, expressed as coordinates in the parameterized configuration space, we need to be able to compute the locations of points on the robot in the physical world.

Example: planar 2R arm

planarArm.png

Consider a simple planar 2R arm. The configuration is given by (\theta_1, \theta_2) \in S \times S. What is the location of the end effector? The location of the first joint is:

(1)
\left( \begin{matrix} {x_1 \\ y_1} \end{matrix} \right) = \left( \begin{matrix} {l_1 \cos \theta_1 \ l_1 \sin \theta_1} \end{matrix} \right)


The location of the end effector is

(2)
\left( \begin{matrix} {x \\ y} \end{matrix} \right) = \left( \begin{matrix} {l_1 \cos \theta_1 + l_2 \cos (\theta_1 + \theta_2) \ l_1 \sin \theta_1 + l_2 \sin(\theta_1 + \theta_2)} \end{matrix} \right)

It's annoying to write \sin(\theta_1 + \theta_2), and what if we have five links?
We use a notational shorthand for sine

(3)
s_{1 2 \ldots n} & = &\sin(\theta_1 + \theta_2 + \ldots + \theta_n)

and have a similar notation for cosine.

With this notation, the location of the effector is:

(4)
\left( \begin{matrix} {x \\ y} \end{matrix} \right) = \left( \begin{matrix} {l_1 c_1 + l_2 c_{12} \ l_1 s_1 + l_2 c_{12}} \end{matrix} \right)

In general, we write the forward kinematics in the form

(5)
\left( \begin{matrix} {x_1 \\ x_2 \\ \vdots \\ x_n} \end{matrix} \right) = f(q)

f(q) is a vector-valued function of a vector: it takes d arguments (where d is the dimension of the configuration space), and gives n values (where n is the dimension of the point whose location we would like to know.)

Sometimes, a forward kinematics map might give information other than just the locations of points. Let's say we want to know the angle of the end effector, as well as its position. (The angle is probably important, since you can't pick something up with the back of your hand!) Write q_e = (x_e, y_e, \theta_e) to be the configuration of the end-effector system. (The end-effector system is a subset of the robot system!)

(6)
q_e = \left( \begin{matrix} {l_1 c_1 + l_2 c_{12} \ l_1 s_1 + l_2 s_{12} \ \theta_1 + \theta_2} \end{matrix} \right)

Forward kinematics using homogeneous coordinates

Consider the following example:

framekinematics.png

We know that frame 1 is rotated by some angle \theta_1 from frame 0, frame 2 is displaced by a distance of l_1 from frame 1, and rotated by an angle of \theta_2. We know that the red point is located at the coordinates (l_2, 0) in frame 2. The forward kinematics problem can therefore be described as: given a sequence of frames and a point described with respect to one of those frames, find the location of that point with respect to the world frame (frame 0).

Let's say you have a world frame, called frame 0, and some other frame, called frame 1, that is rotated from the world frame by 30 degrees, translated a distance of 1 along the x axis, and a distance of .5 along the y axis. We could write a matrix using homogeneous coordinates that describes the location of frame 1 with respect to frame 0:

(7)
^0_1T = \left[ \begin{array}{ccc} \cos 30 & -\sin 30 & 1 \ \sin 30 & \cos 30 & .5 \ 0 & 0 & 1 \end{array} \right].

Let's say you have a point ^1a described with respect to frame 1. Where is the point with respect to frame 0? The superscript before the vector or matrix means "described with respect to frame". That is, given the matrix and ^1a, we want to solve for ^0a.

(8)
^0_1T ^1a = ^0a

Now let's say you have a point ^3b, frame 1 is described with respect to frame 0, frame 2 is described with respect to frame 1, and frame 3 is described with respect to frame 2. What is ^0b, the location of the point in the world frame? Let's first find the location of the point with respect to frame 2:

(9)
^2_3T ^3b = ^2b

Once we have the point with respect to frame 2, we premultiply to move to frame 1, and then again to move to frame 0:

(10)
^0_1T ^1_2T ^2_3T ^3b = ^0b

Notice how we can "cancel" superscripts and subscripts, and because matrix multiplication is associative (though not commutative!) we can premultiply matrices. So for example, the matrix that describes frame 3 with respect to frame 1 is

(11)
^1_3 T = ^1_2T ^2_3T.

Inverse Kinematics

Forward kinematics tells us where a point on a robot arm is, assuming we know the angles at all the joints. This can be useful (especially for checking for things like collisions), but what we probably really want to know is what joint angles to set to reach some given point in space. The mapping from locations in the workspace to points in configuration space is called the "inverse kinematics" of the arm.

For serial manipulators, the inverse kinematics problem is typically much more challenging than the forward kinematics problem. Given a chain of homogeneous transform matrices, and the location of the point, we need to solve for the \theta values, and they are inside the sine and cosines.

Even for our simple 2R planar arm example, solving for \theta_1 and \theta_2 is going to take a little work:

(12)
\left( \begin{matrix} {x \\ y} \end{matrix} \right) = \left( \begin{matrix} {l_1 \cos \theta_1 + l_2 \cos (\theta_1 + \theta_2) \ l_1 \sin \theta_1 + l_2 \sin(\theta_1 + \theta_2)} \end{matrix} \right)

Inverse kinematics for planar 2R arm, geometric approach

The trick is to draw a right triangle inscribing the arm as shown. Then compute the length of the hypotenuse for that triangle: c^2 = x^2 + y^2. Now we can use the law of cosines to compute the angle capital C, by looking at the triangle formed by the hypotenuse and the two links of the arm. We have

(13)
c^2 = l_1^2 + l_2^2 - 2 l_1 l_2 \cos C

Solve for C using an inverse cosine, making sure to get the correct solution for the inverse cosine by inspection. Then \theta_2 = \pi - C.

inverse2r.png

Now notice that we can solve for the angle B using the law of sines on the same triangle. Once we have B, notice that \theta_1 + B = \tan^{-1}(y, x), the two-argument arctangent of the initial point.

Solving transcendental equations by reduction to polynomial

How would you solve the inverse kinematics of a more complicated arm using the law of cosines approach? I would not want to. You could also take an algebraic approach and attempt to directly solve the kinematic equations, and for the 2R arm, such a solution requires only a fair amount of cleverness.

However, there is a nice systematic way to solve problems of this sort. The difficulty with solving transcendental equations is that for each variable \theta_i, both a sine and a cosine term may appear. There's also some issue with the summations inside the sines and cosines.

The summation issue is solved easily for planar arms: measure the angles from the horizontal, rather than with respect to the previous frame — at the end, we can just subtract to get the actual joint angles.

The sines and cosines can be eliminated by observing that if we let u = \tan(\theta/2), then

(14)
\cos \theta = \frac{1 - u^2}{1 + u^2}

and

(15)
\sin \theta = \frac{2u}{1 + u^2}.

Craig's "Introduction to Robotics" text gives the example of solving the equation

(16)
a \cos \theta + b \sin \theta = c.


Making the substitution and multiplying through by 1 + u^2, we get

(17)
a ( 1 - u^2) + 2bu = c(1 + u^2)

which after collecting powers of u is

(18)
(a + c) u^2 - 2 b u + (c - a) =0.

Solve for u with the quadratic formula, and take the inverse tangent to solve for \theta/2.

Inverse kinematics for more complicated arms

Inverse kinematics of complicated arms may need to be solved numerically, since general closed-form solutions are not known to find the roots of polynomials of greater than degree 4. However, certain special cases are known. For example, six-DOF arms are common, since six degrees of freedom allows full control over the pose of a rigid body in space. Work by Pieper solves the inverse kinematics in the special case where three of the joint axes intersect at a point, and in fact, this has influenced the design of real robot arms.

Parallel manipulators

In some sense, the forward kinematics for a serial manipulator are "easy", and the inverse kinematics are "hard". For a parallel manipulator, we expect the opposite to be true.

page_revision: 39, last_edited: 1232978851|%e %b %Y, %H:%M %Z (%O ago)
EditTags History Files Print Site tools+ Options