# L09

Mechanics

Nov 13, 2013 (4 years and 6 months ago)

127 views

ENG4406 ROBOTICS AND MACHINE VISION

KINEMATICS

ANALYSIS

OF

ROBOTS

(Part 2)

PART 2 LECTURE 9

This lecture continues the discussion on the analysis of the
forward and inverse kinematics of robots.

After this lecture, the student should be able to:

Put into practice the concept of inverse kinematics analysis of
robots

Derive inverse transformation matrix between coupled links

Formulate the inverse kinematics of articulated robots in terms of

Solve problems of robot inverse kinematics analysis using
transformation matrices

Kinematics Analysis of Robots II

Inverse Transformation

1
0
0
0
)
cos(
)
cos(
)
cos(
)
sin(
)
sin(
)
sin(
)
sin(
)
sin(
)
cos(
)
cos(
)
sin(
)
cos(
0
)
sin(
)
cos(
1
1
1
1
1
1
1
1
1
1
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
d
d
a
T

A typical transformation matrix can be partitioned as follow:

to give

1
0
0
0
1
P
R
T
i
i
where

)
cos(
)
cos(
)
sin(
)
sin(
)
sin(
)
sin(
)
cos(
)
cos(
)
sin(
)
cos(
0
)
sin(
)
cos(
1
1
1
1
1
1
i
i
i
i
i
i
i
i
i
i
i
i
R

)
cos(
)
sin(
1
1
1
i
i
i
i
i
d
d
a
P

and

Inverse Transformation

1
0
0
0
1
1
P
R
R
T
T
T
i
i
The inverse transformation is given by:

1
1

T
i
i

1
0
0
0
0
1
0
0
0
0
)
cos(
)
sin(
0
0
)
sin(
)
cos(
1
1
1
1
0
1

T
For example, find the inverse of :

1
0
1

T
Inverse Transformation of

1
0
0
0
0
1
0
0
0
0
)
cos(
)
sin(
0
0
)
sin(
)
cos(
1
1
1
1
0
1

T

1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
1
1
1
1
1
1
1
1

T
R
R

0
0
0
0
0
0
1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
0
0
0
1
1
1
1

P
R
P
T
1
0
1

T

1
0
0
0
0
1
0
0
0
0
)
cos(
)
sin(
0
0
)
sin(
)
cos(
1
1
1
1
0
1

T

1
0
0
0
1
1
P
R
R
T
T
T
i
i

1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
1
1
1
1

T
R

0
0
0
P
R
T
Therefore

Inverse Transformation of

1
0
1

T

1
0
0
0
0
1
0
0
0
0
)
cos(
)
sin(
0
0
)
sin(
)
cos(
1
1
1
1
1
0
1

T

1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
2
2
2
2
2
2
2
2

T
R
R

0
)
sin(
)
cos(
0
0
1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
0
0
2
1
2
1
1
2
2
2
2
1

A
A
A
P
R
A
P
T
Find the inverse of

1
0
0
0
0
1
0
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
2
2
1
2
2
1
2

A
T
Inverse Transformation of

1
1
2

T
Therefore

1
0
0
0
0
1
0
0
)
sin(
0
)
cos(
)
sin(
)
cos(
0
)
sin(
)
cos(
2
1
2
2
2
1
2
2
1
1
2

A
A
T

1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
2
2
2
2

T
R

0
)
sin(
)
cos(
2
1
2
1

A
A
P
R
T
and

Inverse Transformation of

1
1
2

T

1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
3
3
3
3
3
3
3
3

T
R
R

0
)
sin(
)
cos(
0
0
1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
0
0
3
2
3
2
2
3
3
3
3
2

A
A
A
P
R
A
P
T
Find the inverse of

1
0
0
0
0
1
0
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
3
3
2
3
3
2
3

A
T
Inverse Transformation of

1
2
3

T
Therefore

1
0
0
0
0
1
0
0
)
sin(
0
)
cos(
)
sin(
)
cos(
0
)
sin(
)
cos(
3
2
3
3
3
2
3
3
1
2
3

A
A
T

1
0
0
0
)
cos(
)
sin(
0
)
sin(
)
cos(
3
3
3
3

T
R

0
)
sin(
)
cos(
3
2
3
2

A
A
P
R
T
and

Inverse Transformation of

1
2
3

T
Inverse Kinematics Problem

Robot forward kinematics involve finding the gripper position and
orientation given the angles of rotation of the linkages

Now consider the following problem for the planar robot:

The arm moves to a new location and the position along with the
orientation of the gripper w.r.t. frame {0} is known. Can you
determine the total rotations of

1
,

2
, and

3

required for the arm
to reach that new position?

This problem is called the robot inverse kinematics problem.

Inverse Kinematics of the Planar Robot

The inverse kinematics problem for the planar robot can be stated
as follow:

Given the global gripper orientation and position, i.e. given

1
0
0
0
0
3
z
z
z
z
y
y
y
y
x
x
x
x
p
a
o
n
p
a
o
n
p
a
o
n
T
Find the joint angles

1
,

2
, and

3

required for the arm to
reach that new position

Inverse Kinematics

To solve the inverse kinematics problem, we know that:

T
T
T
T
p
a
o
n
p
a
o
n
p
a
o
n
z
z
z
z
y
y
y
y
x
x
x
x
0
3
2
3
1
2
0
1
1
0
0
0

1
0
0
0
0
1
0
0
)
sin(
)
sin(
0
)
cos(
)
sin(
)
cos(
)
cos(
0
)
sin(
)
cos(
2
1
2
1
1
3
2
1
3
2
1
2
1
2
1
1
3
2
1
3
2
1
0
3

A
A
A
A
T
Or

We can equate the elements in the above two matrices to try to
determine the joint angles

1
0
0
0
0
1
0
0
)
sin(
)
sin(
0
)
cos(
)
sin(
)
cos(
)
cos(
0
)
sin(
)
cos(
1
0
0
0
2
1
2
1
1
3
2
1
3
2
1
2
1
2
1
1
3
2
1
3
2
1

A
A
A
A
p
a
o
n
p
a
o
n
p
a
o
n
z
z
z
z
y
y
y
y
x
x
x
x
Inverse Kinematics example

Let

=

1
+

2
+

3
.

Equate elements (1,1) and (2,1):

)
cos(
)
cos(
3
2
1

x
n
)
sin(
)
sin(
3
2
1

y
n
Provided that

1
)
(
2
2

y
x
n
n
We can find

using

=arctan(
n
y
, n
x
):

x
y
n
n
1
3
2
1
tan
)
(

)
cos(
)
sin(
)
tan(

Inverse Kinematics example

Next, equate elements (1,4) and (2,4):

)
cos(
)
cos(
2
1
2
1
1

A
A
p
x
)
sin(
)
sin(
2
1
2
1
1

A
A
p
y
Square both of these equations to get:

)
cos(
)
cos(
2
)
(
cos
)
(
cos
2
1
1
2
1
2
1
2
2
2
1
2
2
1
2

A
A
A
A
p
x
)
sin(
)
sin(
2
)
(
sin
)
(
sin
2
1
1
2
1
2
1
2
2
2
1
2
2
1
2

A
A
A
A
p
y
Adding the above two equation yields:

)
cos(
2
2
2
1
2
2
2
1
2
2

A
A
A
A
p
p
y
x

2
1
2
2
2
1
2
2
2
2
)
cos(
A
A
A
A
p
p
y
x

)
cos(
)
sin(
tan
)
(
cos
1
)
sin(
)
(
cos
1
)
(
sin
2
2
1
2
2
2
2
2
2
2
2

Inverse Kinematics example

To solve for

1
, we again reuse the following equations
:

)
sin(
)
cos(
)
cos(
)
cos(
1
1
2
1
2
1
1

A
A
p
x
)
cos(
)
sin(
)
sin(
)
sin(
1
1
2
1
2
1
1

A
A
p
y
where

)
sin(
)
cos(
2
2
2
2
1

A
A
A

Let

)
sin(
)
cos(

r
r

1
2
2
tan
r
where

and

can be found as

2

was previously determined.

Substitute
r

and

into
p
x

and
p
y

to get:

)
sin(
)
sin(
)
cos(
)
cos(
1
1

r
p
x
)
cos(
)
sin(
)
sin(
)
cos(
1
1

r
p
y
r
p
r
p
y
x

)
sin(
)
cos(
1
1

Inverse Kinematics example

We can then find

1

using the known

:

1
1
1
1
tan
tan
tan
x
y
x
y
p
p
p
p

x
y
p
p
1
1
1
1
1
tan
)
cos(
)
sin(
tan
)
(

Summary of inverse kinematics for the planar robot:

1) Find

=

1
+

2
+

3

from

x
y
n
n
1
3
2
1
tan
)
(

2) Find

2
from

)
(
cos
1
)
sin(
,
2
)
cos(
2
2
2
2
1
2
2
2
1
2
2
2

A
A
A
A
p
p
y
x
3) Find

1
from

4) Find

3
from

3
=

-
(

1
+

2
)

1
1
1
1
tan
tan
tan
x
y
x
y
p
p
p
p
Inverse Kinematics exercise 1

The gripper position and orientation for the planar robot is at:

1
0
0
0
0
1
0
0
0
1
0
0
0
1
1
0
0
0
1
2
A
A
p
a
o
n
p
a
o
n
p
a
o
n
z
z
z
z
y
y
y
y
x
x
x
x
Where
A
1
=3, A
2
=2
. Find the joint angles and hence determine
the robot configuration.

0
0
tan
tan
)
(
1
1
3
2
1

x
y
n
n

90
)
cos(
)
sin(
tan
1
)
(
cos
1
)
sin(
0
2
)
cos(
2
2
1
2
2
2
2
2
1
2
2
2
1
2
2
2

A
A
A
A
p
p
y
x
Inverse Kinematics exercise 1

Assuming an elbow down configuration for link 2:

90
2

X
2

Y
2

2

Remember, positive

2

means clockwise rotation as follow:

0
)
(
2
1
3

Results:

0
90
90
3
2
1

3
2
2
)
sin(
3
)
cos(
2
2
1
2
2
1

y
x
p
p
A
A
A
A

90
tan
tan
tan
1
1
1
1

x
y
x
y
p
p
p
p
Inverse Kinematics exercise 1

After

2

has been found:

Visualization of Inverse Kinematics exercise 1

To visualize the arm movement, the robot should looks like this
after rotating

1

= 90
°

(

1

is the angle from X
0

to X
1

measured
along Z
1
).

Y
1

X
1

X
0

Y
0
,

X
2

Y
2

X
3

Y
3

1
=90
°

To visualize the arm movement, the robot should looks like this
after rotating

1

= 90
°

(

1

is the angle from X
0

to X
1

measured
along Z
1
),

and after rotating

2

=
-
90
°

(

2

is the
angle from X
1

to X
2

measured along Z
2
)

Note that

3

= 0
°

2
=
-
90
°

Y
1

X
1

X
0

Y
0
,

X
2

Y
2

X
3

Y
3

Visualization of Inverse Kinematics exercise 1

The orientation of the gripper is as follow:

X
3

is in the positive X
0

direction

T
n
0
0
1

Y
3

is in the positive Y
0

direction

T
o
0
1
0

Z
3

is in the positive Z
0

direction

T
a
1
0
0

Y
1

X
1

X
0

Y
0
,

X
2

Y
2

X
3

Y
3

A
1

A
2

T
A
A
p
0
1
2

Location of gripper in frame {0}

1
0
0
0
0
1
0
0
0
1
0
0
0
1
1
0
0
0
1
2
A
A
p
a
o
n
p
a
o
n
p
a
o
n
z
z
z
z
y
y
y
y
x
x
x
x
Visualization of Inverse Kinematics exercise 1

Inverse Kinematics exercise 2

The gripper position and orientation for the planar robot is at:

1
0
0
0
0
1
0
0
5
0
0
1
0
0
1
0
1
0
0
0
z
z
z
z
y
y
y
y
x
x
x
x
p
a
o
n
p
a
o
n
p
a
o
n
Find the joint angles and hence determine the robot configuration.

90
0
1
tan
tan
)
(
1
1
3
2
1

x
y
n
n

0
)
cos(
)
sin(
tan
0
)
(
cos
1
)
sin(
1
2
)
cos(
2
2
1
2
2
2
2
2
1
2
2
2
1
2
2
2

A
A
A
A
p
p
y
x

0
)
(
2
1
3

Results:

0
0
90
3
2
1

5
0
0
)
sin(
13
)
cos(
2
2
1
2
2
1

y
x
p
p
A
A
A
A

90
tan
tan
tan
1
1
1
1

x
y
x
y
p
p
p
p
Inverse Kinematics exercise 2

After

2

has been found:

To visualize the arm movement, the robot should finally looks
like this after rotating

1

= 90
°

(

1

is the angle from X
0

to X
1

measured along Z
1
),

2

= 0
°

(

2

is the angle from X
1

to X
2

measured along Z
2
) and

3

= 0
°
.

1
=90
°

Y
1

X
1

X
0

Y
0
,

X
2

Y
2

X
3

Y
3

Visualization of Inverse Kinematics exercise 2

Inverse Kinematics

the general approach

For a more complex robot with n>3 links, it is generally not
possible to solve for all the joint variables by just equating the
elements in

To solve for the other joint angles, it may be necessary to find

T
p
a
o
n
p
a
o
n
p
a
o
n
n
z
z
z
z
y
y
y
y
x
x
x
x
0
1
0
0
0

1
0
0
0
1
0
1
2
3
1
2
2
3
1
2
0
1
1
0
1
0
3
1
0
1
z
z
z
z
y
y
y
y
x
x
x
x
p
a
o
n
p
a
o
n
p
a
o
n
T
T
T
T
T
T
T
T
T
The elements in the above equation are compared to solve for
the other joint angles. It may be necessary to repeat the process
with premultiplication of the LHS and RHS with more inverse
transformation matrices.

Inverse Kinematics

Summary

Robot inverse kinematics involve finding the joint variables
given the robot arm global position and orientation

This problem can be solved by equating the overall
transformation matrix with the matrix containing the given
information and comparing their elements

The process may need to be repeated with premultiplication
of the above equation with inverse transformation matrices

Although we have use a planar robot to illustrate the
concept, the approach can be applied to any robot moving
in 3
-
D space.

The approach involves the following:

1.

Solve for joint variables by comparing elements in

Inverse Kinematics

Summary

T
p
a
o
n
p
a
o
n
p
a
o
n
n
z
z
z
z
y
y
y
y
x
x
x
x
0
1
0
0
0

2.
Repeat step 1 with

,
1
0
0
0
1
0
1
2

z
z
z
z
y
y
y
y
x
x
x
x
n
p
a
o
n
p
a
o
n
p
a
o
n
T
T

1
0
0
0
1
0
1
1
1
2
3
z
z
z
z
y
y
y
y
x
x
x
x
n
p
a
o
n
p
a
o
n
p
a
o
n
T
T
T
etc. until all the joint variables are obtained

Summary

This lecture continues the discussion on the analysis of the
forward and inverse kinematics of robots.

The following were covered:

The concept of inverse kinematics analysis of robots

Inverse transformation matrix between coupled links

Inverse kinematics of articulated robots in terms of the link
transformation matrices

Robot inverse kinematics analysis using transformation matrices