Robot Kinematics
S Vetrivel
We are interested in two kinematics topics
Forward Kinematics (angles to position)
What you are given: The length of each link
The angle of each joint
What you can find: The position of any point
(i.e. its (x, y, z) coordinates
Inverse Kinematics (position to angles)
What you are given: The length of each link
The position of some point on the robot
What you can find: The angles of each joint needed to obtain
that position
F o r w a r d K i n e m a t i c s
The Situation:
You have a robotic arm that
starts out aligned with the x
o
-axis.
You tell the first link to move by
1
and the second link to move by
2
.
The Quest:
What is the position of the
end of the robotic arm?
Solution:
1. Geometric Approach
This might be the easiest solution for the simple situation. However,
notice that the angles are measured relative to the direction of the previous
link. (The first link is the exception. The angle is measured relative to its
initial position.) For robots with more links and whose arm extends into 3
dimensions the geometry gets much more tedious.
2. Algebraic Approach
Involves coordinate transformations.
X
2
X
3
Y
2
Y
3
3
1
2 3
Example Problem:
You are have a three link arm that starts out aligned in the x-axis.
Each link has lengths l
1
, l
2
, l
3
, respectively. You tell the first one to move by
1
, and so on as the diagram suggests. Find the Homogeneous matrix to get
the position of the yellow dot in the X
0
Y
0
frame.
H = R
z
(
1
) * T
x1
(l
1
) * R
z
(
2
) * T
x2
(l
2
) * R
z
(
3
)
i.e. Rotating by
1
will put you in the X
1
Y
1
frame.
Translate in the along the X
1
axis by l
1
.
Rotating by
2
will put you in the X
2
Y
2
frame.
and so on until you are in the X
3
Y
3
frame.
The position of the yellow dot relative to the X
3
Y
3
frame is
(l
1
, 0). Multiplying H by that position vector will give you the
coordinates of the yellow point relative the the X
0
Y
0
frame.
X
1
Y
1
X
0
Y
0
Slight variation on the last solution:
Make the yellow dot the origin of a new coordinate X
4
Y
4
frame
X
2
X
3
Y
2
Y
3
3
1
2 3
X
1
Y
1
X
0
Y
0
X
4
Y
4
H = R
z
(
1
) * T
x1
(l
1
) * R
z
(
2
) * T
x2
(l
2
) * R
z
(
3
) * T
x3
(l
3
)
This takes you from the X
0
Y
0
frame to the X
4
Y
4
frame.
The position of the yellow dot relative to the X
4
Y
4
frame
is (0,0).
1
1
1
1
]
1
1
1
1
1
]
1
1
0
0
0
H
1
Z
Y
X
Notice that multiplying by the (0,0,0,1) vector will
equal the last column of the H matrix.
I n v e r s e K i n e m a t i c s
From Position to Angles
A Simple Example
1
X
Y
S
Revolute and
Prismatic Joints
Combined
(x , y)
Finding :
)
x
y
arctan(
More Specifically:
)
x
y
( 2 arctan
arctan2() specifies that its in the
first quadrant
Finding S:
) y (x S
2 2
+
1
(x , y)
l
2
l
1
Inverse Kinematics of a Two Link Manipulator
Given: l
1
, l
2
, x , y
Find:
1
,
2
Redundancy:
A unique solution to this problem
does not exist. Notice, that using the
givens two solutions are possible.
Sometimes no solution is possible.
(x , y)
l
2
l
1
l
2
l
1
The Geometric Solution
l
1
l
2
,
_
+ +
+
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 2 2
2
arccos
2
) cos(
) cos( ) 180 cos(
) 180 cos( 2 ) (
cos 2
l l
l l y x
l l
l l y x
l l l l y x
C ab b a c
2
2
2 2
2
Using the Law of Cosines:
,
_
+
+
x
y
2 arctan
y x
) sin(
y x
) sin(180 sin
sin sin
1 1
2 2
2
2 2
2
2
1
l
c
C
b
B
,
_
,
_
x
y
2 arctan
y x
) sin(
arcsin
2 2
2 2
1
l
Redundant since
2
could be in the
first or fourth quadrant.
Redundancy caused since
2
has two possible
values
( ) ( )
( )
,
_
+
+ +
+ + +
+ + + + +
+ +
+ +
+ + + +
2 1
2
2
2
1
2 2
2
2 2 1
2
2
2
1
2 1 1 2 1 1 2 1
2
2
2
1
2 1 1 2 1
2
2 1
2
2
2
1
2
1 2 1 1 2 1
2
2 1
2
2
2
1
2
1
2 2 2 2
2
y x
arccos
c 2
) (sin s ) (c c 2
) (sin s 2 ) (sin s ) (c c 2 ) (c c
y x ) 2 ( (1)
l l
l l
l l l l
l l l l
l l l l l l l l
The Algebraic Solution
l
1
l
2
(x , y)
2 1
2 1 2 1 1
2 1 2 1 1
1 2 2 1
1 1
(3)
sin s y (2)
c c x (1)
) cos( c
cos c
+
+
+
+
+
+
+
l l
l l
Only Unknown
) )(sin (cos ) )(sin (cos ) sin(
) )(sin (sin ) )(cos (cos ) cos(
:
a b b a b a
b a b a b a
Note
+
+
+
+
+
) c ( s ) s ( c
c s c s s
sin s y
) ( ) c ( c
c c c
c c x
2 2 1 1 2 2 1
1 2 2 2 1 2 1 1
2 1 2 1 1
2 2 1 2 2 1 1
2 1 2 2 1 2 1 1
2 1 2 1 1
l l l
l l l
l l
s l s l l
s s l l l
l l
+ +
+ +
+
+
+
+
+
+
We know what
2
is from the
previous slide. We need to solve for
1
. Now we have two equations and
two unknowns (sin
1
and cos
1
)
( )
2 2
2 2 2 2 1
1
2 2 1
2
2
2
1 1 2 2
2 2 1
2 2 1 1 2 2
2 2 1
2 2 1
2 2 1
2 2 1
1
y x
x ) c ( y
s
) c 2 ( s x
) c (
1
) c ( s ) s (
) c (
) ( x
y
) c (
) ( x
c
+
+
+ + +
+
+ +
+
+
+
+
s l l l
l l l l s l
l l
l l l
l l
s l s
l l
s l s
Substituting for c
1
and simplifying
many times
Notice this is the law of cosines
and can be replaced by x
2
+ y
2
,
_
+
+
2 2
2 2 2 2 1
1
y x
x ) c ( y
arcsin
s l l l