You are on page 1of 46

ME 8209

Robot Kinematics
(Forward and Inverse)
ME 8209
Forward kinematics introduction
Challenge: given all the joint parameters of a manipulator, determine
the position and orientation of the tool frame
Tool frame: coordinate frame attached to the most distal link of the
manipulator
Inertial frame: fixed (immobile) coordinate system fixed to the most proximal
link of a manipulator
Therefore, we want a mapping between the tool frame and the inertial
frame
This will be a function of all joint parameters and the physical geometry of
the manipulator
Purely geometric: we do not worry about joint torques or dynamics
(yet!)


ME 8209
Convention
A n-DOF manipulator will have n joints (either revolute or prismatic) and
n+1 links (since each joint connects two links)
We assume that each joint only has one DOF. Although this may seem like
it does not include things like spherical or universal joints, we can think of
multi-DOF joints as a combination of 1DOF joints with zero length between
them
The o
0
frame is the inertial frame
o
n
is the tool frame
Joint i connects links i-1 and i
The o
i
is connected to link i
Joint variables, q
i

=
prismatic is joint if
revolute is joint if
i d
i
q
i
i
i
u
ME 8209
Convention
We said that a homogeneous transformation allowed us to express the
position and orientation of o
j
with respect to o
i
what we want is the position and orientation of the tool frame with respect to
the inertial frame
An intermediate step is to determine the transformation matrix that gives
position and orientation of o
i
with respect to o
i-1
: A
i
Now we can define the transformation o
j
to o
i
as:


( ) i j
j i
j i
T
I
A A A A
T
j
i
j i j i i
i
j
>
=
<

+ +
if
if
if

1
2 1
...
ME 8209
Convention
Finally, the position and orientation of the tool frame with respect to the
inertial frame is given by one homogeneous transformation matrix:
For a n-DOF manipulator



Thus, to fully define the forward kinematics for any serial manipulator,
all we need to do is create the A
i
transformations and perform matrix
multiplication

But there are shortcuts

( ) ( ) ( )
n n n
n n
q A q A q A T
o R
H = =
(

=
2 2 1 1
0
0 0
1 0
ME 8209
The Denavit-Hartenberg (DH) Convention
Representing each individual homogeneous transformation as the
product of four basic transformations:

(
(
(
(

=
(
(
(
(

(
(
(
(

(
(
(
(

(
(
(
(


=
=
1 0 0 0
0
1 0 0 0
0 0
0 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0 1 0
0 0 1
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0
0 0
, , , ,
i
i
i
i
i
x a x d z z i
d c s
s a s c c c s
c a s s c s c
c s
s c
a
d
c s
s c
A
i i
i i i i i i
i i i i i i
i i
i i i i
i i
i i i i
o o
u o u o u u
u o u o u u
o o
o o u u
u u
o u
Rot Trans Trans Rot
ME 8209
The Denavit-Hartenberg (DH) Convention
Four DH parameters:
a
i
: link length
o
i
: link twist
d
i
: link offset
u
i
: joint angle
Since each A
i
is a function of only one variable, three of these will be
constant for each link
d
i
will be variable for prismatic joints and u
i
will be variable for revolute joints

But we said any rigid body needs 6 parameters to describe its position
and orientation
Three angles (Euler angles, for example) and a 3x1 position vector
So how can there be just 4 DH parameters?...

ME 8209
Existence and uniqueness
When can we represent a homogeneous transformation using the 4 DH
parameters?
For example, consider two coordinate frames o
0
and o
1
There is a unique homogeneous transformation between these two frames
Now assume that the following holds:
1. DH1:
2. DH2:
If these hold, we claim that there
exists a unique transformation A:
0 1

z x
0 1

z x
(

=
=
1 0
0
1
0
1
, , , ,
o R
A
x a x d z z o u
Rot Trans Trans Rot
ME 8209
Existence and uniqueness
Proof:
1. We assume that R
1
0
has the form:
2. Use DH1 to verify the form of R
1
0








Since the rows and columns of R
1
0
must be unit vectors:
The remainder of R
1
0
follows from the properties of
rotation matrices
Therefore our assumption that there exists a unique u
and o that will give us R
1
0
is correct given DH1

0
1
0
0
0

31
31
21
11
0
0
0
1 0 1
= =
(
(
(

(
(
(

=
r
r
r
r
z x z x
T
o u , ,
0
1 x z
R R R =
(
(
(

=
33 32
23 22 21
13 12 11
0
1
0 r r
r r r
r r r
R
1
1
2
33
2
32
2
21
2
11
= +
= +
r r
r r
ME 8209
Existence and uniqueness
Proof:
1. Use DH2 to determine the form of o
1
0
Since the two axes intersect, we can represent the line between the two
frames as a linear combination of the two axes (within the plane formed by
x
1
and z
0
)

(
(
(

=
(
(
(

+
(
(
(

=
+ =
d
as
ac
s
c
a d o
ax dz o z x
u
u
u
u
0 1
0
0

0
1
0
1
0
0
0
1 0 1
ME 8209
Physical basis for DH parameters
a
i
: link length, distance between the z
0
and z
1
(along x
1
)
o
i
: link twist, angle between z
0
and z
1
(measured around x
1
)
d
i
: link offset, distance between o
0
and intersection of z
0
and x
1
(along z
0
)
u
i
: joint angle, angle between x
0
and x
1
(measured around z
0
)


positive convention:
ME 8209
Assigning coordinate frames
For any n-link manipulator, we can always choose coordinate frames
such that DH1 and DH2 are satisfied
The choice is not unique, but the end result will always be the same
1. Choose z
i
as axis of rotation for joint i+1
z
0
is axis of rotation for joint 1, z
1
is axis of rotation for joint 2, etc
If joint i+1 is revolute, z
i
is the axis of rotation of joint i+1
If joint i+1 is prismatic, z
i
is the axis of translation for joint i+1
ME 8209
Assigning coordinate frames
2. Assign base frame
Can be any point along z
0
3. Chose x
0
, y
0
to follow the right-handed convention
4. Now start an iterative process to define frame i with respect to frame i-1
Consider three cases for the relationship of z
i-1
and z
i
:
i. z
i-1
and z
i
are non-coplanar
ii. z
i-1
and z
i
intersect
iii. z
i-1
and z
i
are parallel

z
i-1
and z
i
are coplanar
ME 8209
Assigning coordinate frames
i. z
i-1
and z
i
are non-coplanar
There is a unique shortest distance between the two axes
Choose this line segment to be x
i
o
i
is at the intersection of z
i
and x
i
Choose y
i
by right-handed convention
ii. z
i-1
and z
i
intersect
Choose x
i
to be normal to the plane defined by z
i
and z
i-1
o
i
is at the intersection of z
i
and x
i
Choose y
i
by right-handed convention
iii. z
i-1
and z
i
are parallel
Infinitely many normals of equal length between z
i
and z
i-1
Free to choose o
i
anywhere along z
i
, however if we choose x
i
to be along
the normal that intersects at o
i-1
, the resulting d
i
will be zero
Choose y
i
by right-handed convention

ME 8209
Assigning tool frame
The previous assignments are valid up to frame n-1
The tool frame assignment is most often defined by the axes n, s, a:
a is the approach direction
s is the sliding direction (direction along which the grippers open/close)
n is the normal direction to a and s
ME 8209
Summary - General procedure for determining fwd. kinematics
1. Label joint axes as z
0
, , z
n-1
(axis z
i
is joint axis for joint i+1)
2. Choose base frame: set o
0
on z
0
and choose x
0
and y
0
using right-
handed convention
3. For i=1:n-1,
i. Place o
i
where the normal to z
i
and z
i-1
intersects z
i
. If z
i
intersects z
i-1
, put
o
i
at intersection. If z
i
and z
i-1
are parallel, place o
i
along z
i
such that d
i
=0
ii. x
i
is the common normal through o
i
, or normal to the plane formed by z
i-1

and z
i
if the two intersect
iii. Determine y
i
using right-handed convention
4. Place the tool frame: set z
n
parallel to z
n-1
5. For i=1:n, fill in the table of DH parameters
6. Form homogeneous transformation matrices, A
i
7. Create T
n
0
that gives the position and orientation of the end-effector in
the inertial frame
ME 8209
Example 1: two-link planar manipulator
2DOF: need to assign three coordinate frames
1. Choose z
0
axis (axis of rotation for joint 1, base frame)
2. Choose z
1
axis (axis of rotation for joint 2)
3. Choose z
2
axis (tool frame)
This is arbitrary for this case since we have described no wrist/gripper
Instead, define z
2
as parallel to z
1
and z
0
(for consistency)
4. Choose x
i
axes
All z
i
s are parallel
Therefore choose x
i
to intersect o
i-1


ME 8209
Example 1: two-link planar manipulator
Now define DH parameters
First, define the constant parameters a
i
, o
i

Second, define the variable parameters u
i
, d
i





The o
i
terms are 0 because all z
i
are parallel
Therefore only u
i
are variable


link a
i
o
i
d
i
u
i
1 a
1
0 0 u
1
2 a
2
0 0 u
2
(
(
(
(


=
(
(
(
(


=
1 0 0 0
0 1 0 0
0
0
1 0 0 0
0 1 0 0
0
0
2 2 2 2
2 2 2 2
2
1 1 1 1
1 1 1 1
1
s a c s
c a s c
A
s a c s
c a s c
A ,
(
(
(
(

+
+
= =
=
1 0 0 0
0 1 0 0
0
0
12 2 1 1 12 12
12 2 1 1 12 12
2 1
0
2
1
0
1
s a s a c s
c a c a s c
A A T
A T
ME 8209
Example 2: three-link cylindrical robot
3DOF: need to assign four coordinate frames
1. Choose z
0
axis (axis of rotation for joint 1, base frame)
2. Choose z
1
axis (axis of translation for joint 2)
3. Choose z
2
axis (axis of translation for joint 3)
4. Choose z
3
axis (tool frame)
This is again arbitrary for this case since we have described no wrist/gripper
Instead, define z
3
as parallel to z
2

ME 8209
Example 2: three-link cylindrical robot
Now define DH parameters
First, define the constant parameters a
i
, o
i

Second, define the variable parameters u
i
, d
i






link a
i
o
i
d
i
u
i
1 0

0 d
1
u
1
2 0

-90 d
2
0

3 0 0 d
3
0
(
(
(
(

=
(
(
(
(

=
(
(
(
(


=
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0
0 1 0 0
0 0 0 1
1 0 0 0
1 0 0
0 0
0 0
3
3
2
2
1
1 1
1 1
1
d
A
d
A
d
c s
s c
A , ,

(
(
(
(

+

= =
1 0 0 0
0 1 0
0
0
2 1
3 1 1 1
3 1 1 1
3 2 1
0
3
d d
d c c s
d s s c
A A A T
ME 8209
Example 3: spherical wrist
3DOF: need to assign four coordinate frames
yaw, pitch, roll (u
4
, u
5
, u
6
) all intersecting at one point o (wrist center)
ME 8209
Example 3: spherical wrist
link a
i
o
i
d
i
u
i
4 0

-90 0

u
4
5 0

90 0 u
5
6 0 0 d
6
u
6
Now define DH parameters
First, define the constant parameters a
i
, o
i

Second, define the variable parameters u
i
, d
i




(
(
(
(


=
(
(
(
(

=
(
(
(
(

=
1 0 0 0
1 0 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
6
6 6
6 6
3
5 5
5 5
2
4 4
4 4
1
d
c s
s c
A
c s
s c
A
c s
s c
A , ,

(
(
(
(

+ +

= =
1 0 0 0
6 5 5 6 5 6 5
6 5 4 5 4 6 4 6 5 4 6 4 6 5 4
6 5 4 5 4 6 4 6 5 4 6 4 6 5 4
6 5 4
3
6
d c c c s c s
d s s s s c c s c s s c c c s
d s c s c c s s c c s s c c c
A A A T
ME 8209
Example 4: cylindrical robot with spherical wrist
6DOF: need to assign seven coordinate frames
But we already did this for the previous two examples, so we can fill in the
table of DH parameters:

link a
i
o
i
d
i
u
i
1 0 0 d
1
u
1
2 0 -90 d
2
0
3 0 0 d
3
0
4 0

-90 0

u
4
5 0

90 0 u
5
6 0 0 d
6
u
6
o
3
, o
4
, o
5
are all at
the same point o
c

ME 8209
Example 4: cylindrical robot with spherical wrist
Note that z
3
(axis for joint 4) is collinear with z
2
(axis for joint 3), thus we
can make the following combination:

(
(
(
(

= =
1 0 0 0
33 32 31
23 22 21
13 12 11
3
6
0
3
0
6
z
y
x
d r r r
d r r r
d r r r
T T T
2 1 6 5 4
3 1 6 5 1 6 5 4 1
3 1 6 5 1 6 5 4 1
5 4 33
5 1 5 4 1 23
5 1 5 4 1 13
6 4 6 5 4 32
6 5 1 6 4 1 6 5 4 1 22
6 5 1 6 4 1 6 5 4 1 12
6 4 6 5 4 31
6 5 1 6 4 1 6 5 4 1 21
6 5 1 6 4 1 6 5 4 1 11
d d d s s d
d c d c c d s c s d
d s d c s d s c c d
s s r
c c s c s r
c s s c c r
c c c c s r
c s c s s s s c c s r
c s s c s c s c c c r
s c c c s r
c s c s s s c c c s r
c s s s s c c c c c r
z
y
x
+ + =
+ + =
=
=
+ =
=
=
+ =
=
=
=
+ =
ME 8209
Example 5: the Stanford manipulator
6DOF: need to assign seven coordinate frames:
1. Choose z
0
axis (axis of rotation for joint 1, base frame)
2. Choose z
1
-z
5
axes (axes of rotation/translation for joints 2-6)
3. Choose x
i
axes
4. Choose tool frame
5. Fill in table of DH parameters:
link a
i
o
i
d
i
u
i
1 0 -90 0

u
1
2 0 90 d
2
u
2
3 0 0 d
3
0
4 0

-90 0

u
4
5 0

90 0 u
5
6 0 0 d
6
u
6
ME 8209
Example 5: the Stanford manipulator
Now determine the individual homogeneous transformations:
(
(
(
(


=
(
(
(
(


=
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
1 0 0 0
1 0 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
6
6 6
6 6
6
5 5
5 5
5
4 4
4 4
4
3
3
2
2 2
2 2
2
1 1
1 1
1
d
c s
s c
A
c s
s c
A
c s
s c
A
d
A
d
c s
s c
A
c s
s c
A
, ,
, ,
ME 8209
Example 5: the Stanford manipulator
Finally, combine to give the complete description of the forward
kinematics:

(
(
(
(

= =
1 0 0 0
33 32 31
23 22 21
13 12 11
6 1
0
6
z
y
x
d r r r
d r r r
d r r r
A A T
( ) | | ( )
( ) | | ( )
( )
( ) | | ( )
( ) | | ( )
( )
( )
( )
( )
( )
( )
5 2 4 5 2 6 3 2
2 1 5 5 1 4 2 5 4 1 6 2 1 3 2 1
5 4 1 2 5 1 5 4 2 1 6 2 1 3 2 1
5 2 5 4 2 33
5 4 1 5 2 5 4 2 1 23
5 4 1 5 2 5 4 2 1 13
6 5 2 6 4 6 5 4 2 32
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 22
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 12
6 5 2 6 4 6 5 4 2 31
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 21
6 4 6 5 4 2 6 5 2 6 4 6 5 4 2 1 11
s s c c c d d c d
s s c s s c c s s c d d c d s s d
s s s s c c s c c c d d s d s c d
c c s c s r
s s c c s s c c s r
s s s c s s c c c r
s s c c s s c c s r
s c s c s c s s s c s s c c c s r
c c s c s s s s s c s s c c c c r
c s c s s c c c s r
s c c c s c c s s s s c c c c s r
s c c c s d c s s s s c c c c c r
z
y
x
+ =
+ + + + =
+ + =
+ =
+ + =
+ =
+ + =
+ + =
+ + + =
=
+ + =
+ =
ME 8209
Example 6: the SCARA manipulator
4DOF: need to assign five coordinate frames:
1. Choose z
0
axis (axis of rotation for joint 1, base frame)
2. Choose z
1
-z
3
axes (axes of rotation/translation for joints 2-4)
3. Choose x
i
axes
4. Choose tool frame
5. Fill in table of DH parameters:
link a
i
o
i
d
i
u
i
1 a
1
0 0

u
1
2 a
2
180 0

u
2
3 0 0 d
3
0
4 0

0 d
4
u
4
ME 8209
Example 6: the SCARA manipulator
Now determine the individual homogeneous transformations:
(
(
(
(


=
(
(
(
(

=
(
(
(
(

=
(
(
(
(


=
1 0 0 0
1 0 0
0 0
0 0
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
0
0
1 0 0 0
0 1 0 0
0
0
4
4 4
4 4
4
3
3
2 2 2 2
2 2 2 2
2
1 1 1 1
1 1 1 1
1
d
c s
s c
A
d
A
s a c s
c a s c
A
s a c s
c a s c
A , , ,

(
(
(
(


+
+ + +
= =
1 0 0 0
1 0 0
0
0
4 3
12 2 1 1 4 12 4 12 4 12 4 12
12 2 1 1 4 12 4 12 4 12 4 12
4 1
0
4
d d
s a s a c c s s s c c s
c a c a c s s c s s c c
A A T
ME 8209
Forward kinematics of parallel manipulators
Parallel manipulator: two or more series chains connect the end-
effector to the base (closed-chain)
# of DOF for a parallel manipulator determined by taking the total DOFs
for all links and subtracting the number of constraints imposed by the
closed-chain configuration
Grueblers formula (3D):
( )

=
+ =
j
n
i
i j L
f n n
1
6 DOF #
number of links*
*excluding ground
number of joints
#DOF for joint i
ME 8209
Forward kinematics of parallel manipulators
Grueblers formula (2D):

Example (2D):
Planar four-bar, n
L
= 3, n
j
= 4, f
i
= 1(for all joints)
3(3-4)+4 = 1DOF
Forward kinematics:

( )

=
+ =
j
n
i
i j L
f n n
1
3 DOF #
( )
2
tan
2
2 2
cos
1
2 1
2
2
2
1
2
2
2
1
t
o
o
o o
u
|
|
.
|

\
|

+
|
|
.
|

\
|
+
+
=

L
L
L L
L
ME 8209
Inverse Kinematics
Find the values of joint parameters that will put the tool frame at a
desired position and orientation (within the workspace)
Given H:


Find all solutions to:

Noting that:

This gives 12 (nontrivial) equations with n unknowns


( ) 3
1 0
SE
o R
H e
(

=
( ) H q q T
n n
= ,...,
1
0
( ) ( ) ( )
n n n n
q A q A q q T =
1 1 1
0
,...,
ME 8209
For a given H:




Find u
1
, u
2
, d
3
, u
4
, u
5
, u
6
:






One solution: u
1
= t/2, u
2
= t/2, d
3
= 0.5, u
4
= t/2, u
5
= 0, u
6
= t/2

(
(
(
(


=
1 0 0 0
0 0 0 1
763 . 0 1 0 0
154 . 0 0 1 0
H
Example: the Stanford manipulator
( ) | | ( )
( ) | | ( )
( )
( ) | | ( )
( ) | | ( )
( )
( )
( )
( )
( )
( ) 0
763 . 0
154 . 0
0
1
0
0
0
1
1
0
0
5 2 4 5 2 6 3 2
2 1 5 5 1 4 2 5 4 1 6 2 1 3 2 1
5 4 1 2 5 1 5 4 2 1 6 2 1 3 2 1
5 2 5 4 2
5 4 1 5 2 5 4 2 1
5 4 1 5 2 5 4 2 1
6 5 2 6 4 6 5 4 2
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1
6 5 2 6 4 6 5 4 2
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1
6 4 6 5 4 2 6 5 2 6 4 6 5 4 2 1
= +
= + + + +
= + +
= +
= + +
= +
= + +
= + +
= + + +
=
= + +
= +
s s c c c d d c
s s c s s c c s s c d d c d s s
s s s s c c s c c c d d s d s c
c c s c s
s s c c s s c c s
s s s c s s c c c
s s c c s s c c s
s c s c s c s s s c s s c c c s
c c s c s s s s s c s s c c c c
c s c s s c c c s
s c c c s c c s s s s c c c c s
s c c c s d c s s s s c c c c c
ME 8209
Inverse Kinematics
The previous example shows how difficult it would be to obtain a
closed-form solution to the 12 equations
Instead, we develop systematic methods based upon the manipulator
configuration
For the forward kinematics there is always a unique solution
Potentially complex nonlinear functions
The inverse kinematics may or may not have a solution
Solutions may or may not be unique
Solutions may violate joint limits
Closed-form solutions are ideal!

ME 8209
Overview: kinematic decoupling
Appropriate for systems that have an arm a wrist
Such that the wrist joint axes are aligned at a point
For such systems, we can split the inverse kinematics problem into two
parts:
1. Inverse position kinematics: position of the wrist center
2. Inverse orientation kinematics: orientation of the wrist
First, assume 6DOF, the last three intersecting at o
c




Use the position of the wrist center to determine the first three joint
angles
( )
( ) o q q o
R q q R
=
=
6 1
0
6
6 1
0
6
,...,
,...,
ME 8209
Overview: kinematic decoupling
Now, origin of tool frame, o
6
, is a distance d
6
translated along z
5
(since
z
5
and z
6
are collinear)
Thus, the third column of R is the direction of z
6
(w/ respect to the base
frame) and we can write:


Rearranging:


Calling o = [o
x
o
y
o
z
]
T
, o
c
0
= [x
c
y
c
z
c
]
T
(
(
(

+ = =
1
0
0
6
0
6
R d o o o
o
c
(
(
(

=
1
0
0
6
R d o o
o
c
(
(
(

=
(
(
(

33 6
23 6
13 6
r d o
r d o
r d o
z
y
x
z
y
x
c
c
c
ME 8209
Overview: kinematic decoupling
Since [x
c
y
c
z
c
]
T
are determined from the first three joint angles, our
forward kinematics expression now allows us to solve for the first three
joint angles decoupled from the final three.
Thus we now have R
3
0
Note that:

To solve for the final three joint angles:


Since the last three joints for a
spherical wrist, we can use a set of
Euler angles to solve for them
3
6
0
3
R R R =
( ) ( ) R R R R R
T
0
3
1
0
3
3
6
= =

ME 8209
Inverse position
Now that we have [x
c
y
c
z
c
]
T
we need to find q
1
, q
2
, q
3

Solve for q
i
by projecting onto the x
i-1
, y
i-1
plane, solve trig problem
Two examples: elbow (RRR) and spherical (RRP) manipulators
For example, for an elbow manipulator, to solve for u
1
, project the arm onto
the x
0
, y
0
plane
ME 8209
Background: two argument atan
We use atan2() instead of atan() to account for the full range of
angular solutions
Called four-quadrant arctan
( )
( )

= =
= >
> > |
.
|

\
|
< > |
.
|

\
|

<
=
0 , 0
0 , 0
2
0 , 0
0 , 0
0 , 2
, 2
x y
x y
x y
x
y
x y
x
y
y x y
x y
undefined
atan
atan
atan
atan
t
t
ME 8209
Example: RRR manipulator
1. To solve for u
1
, project the arm onto the x
0
, y
0
plane










Can also have:
This will of course change the solutions for u
2
and u
3
( )
c c
y x , 2
1
atan = u
( )
c c
y x , 2
1
atan + = t u
ME 8209
If there is an offset, then we will
have two solutions for u
1
: left arm
and right arm
However, wrist centers cannot
intersect z
0
If x
c
=y
c
=0, u
1
is undefined
i.e. any value of u
1
will work

Caveats: singular configurations, offsets
ME 8209
Left arm: Right arm:
Left arm and right arm solutions
( )
|
.
|

\
|
+ =
=
=
d d y x
y x
c c
c c
, 2
, 2
2
2 2
1
atan
atan
-
o
|
o | u
( )
|
.
|

\
|
+ =
|
.
|

\
|
+ + =
=
+ =
d d y x
d d y x
y x
c c
c c
c c
, 2
, 2
, 2
2
2 2
2
2 2
1
atan
atan
atan
t |
o
| o u
ME 8209
Therefore there are in general two solutions for u
1
Finding u
2
and u
3
is identical to the planar two-link manipulator we have
seen previously:






Therefore we can find two solutions for u
3
:
Left arm and right arm solutions
( )
D
a a
a a d z d y x
d z s
d y x r
a a
a a s r
c c c
c
c c

+ +
=
=
+ =
+
=
3 2
2
3
2
2
2
1
2
2 2
3
1
2
2 2
2
3 2
2
3
2
2
2 2
3
2
cos
2
cos
u
u
( )
2
3
1 , 2 D D = atan u
ME 8209
The two solutions for u
3
correspond to the elbow-down and elbow-up
positions respectively
Now solve for u
2
:




Thus there are two solutions for the pair (u
2
, u
3
)

Left arm and right arm solutions
( ) ( )
( )
3 3 3 3 2 1
2
2 2
3 3 3 3 2 2
, 2 , 2
, 2 , 2
s a c a a d z d y x
s a c a a s r
c c c
+ |
.
|

\
|
+ =
+ =
atan atan
atan atan u
ME 8209
In general, there will be a maximum of four solutions to the inverse
position kinematics of an elbow manipulator
Ex: PUMA
RRR: Four total solutions
ME 8209
Spherical configuration
Solve for u
1
using same method as with RRR

Again, if there is an offset, there
will be left-arm and right-arm solutions
Solve for u
2
:




Solve for d
3
:


Example: RRP manipulator
( )
c c
y x , 2
1
atan = u
( )
1
2 2
2
2
, 2
d z s
y x r
r s
c
c c
=
+ =
= atan u
( )
2
1
2 2
2 2
3
d z y x
s r d
c c c
+ + =
+ =

You might also like