Inverse Manipulator Inverse Manipulator

Kinematics Kinematics
December 21, 2009 December 21, 2009
Link parameters p
Forward Kinematics
Position and orientation
of the end-effector
Joint angles
of he end effec or
n
, , ,
2 1
L
Link parameters
Position and orientation
J i l
Inverse Kinematics
of the end-effector
Joint angles
n
, , ,
2 1
L
Inverse Kinematics Inverse Kinematics
Given the desired position and orientation of
the tool relative to the station, how do we
compute the set of joint angles which will p j g
achieve this desired result?
First fr me tr nsform tions re performed to First, frame transformations are performed to
find the wrist frame, {W}, relative to the base
frame, {B}, and then the inverse kinematics are
used to solve for the joint angles. j g
Solvability Solvability
T
N
0
L
Given the numerical value of we attempt to
find values of . , , ,
2 1 n
find values of
The PUMA 560:
Given as 16 numerical values, solve (3.14) for
6 joint angles,
T
0
6
. , , ,
6 2 1
L
j g ,
12 equations and 6 unknowns q
6 equations and 6 unknowns
Not algebraic,
Exponential, trigonometric functions
q
(nonlinear, transcendental equations)
Solvability Solvability
) ( ) ( ) ( ) ( ) ( ) (
23 22 21
13 12 11
5 4 3 2 1 0 0
p r r r
p r r r
T T T T T T T
y
x

= =
1 0 0 0
) ( ) ( ) ( ) ( ) ( ) (
33 32 31
6 6 5 5 4 4 3 3 2 2 1 1 6
p r r r
T T T T T T T
z

= =
4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 2 1

Forward Kinematics
4 4 4 3 4 4 4 2 1
Goal
( ) [ ] ( ),
21
6 4 6 5 4 1 5 5 23 5 4 6 5 4 23 1 11
r
s c c c s s c s s s s c c c c c r
=
+ + =
L
p
y
=L
M
Independent Equation:
3- Rotation Matrix
.
23 4 2 2 23 3
c d s a s a p
z
y
=
3- Position Vector
Existence of solutions Existence of solutions
In the forward kinematics problem, each set of
input joint parameters gave a unique output
pose. However, in the inverse kinematics, a p , ,
given pose may be satisfied with several
different sets of input angles different sets of input angles.
must be in the workspace of the manipulator.
T
N
0
Workspace Workspace
The volume of the space which the end he volume of the space which the end- -effector effector
of the manipulator can reach. of the manipulator can reach.
Dextrous Dextrous workspace ( workspace (DW DW): reachable with all ): reachable with all Dextrous Dextrous workspace ( workspace (DW DW): reachable with all ): reachable with all
orientations orientations
h bl h bl kk ) h bl l ) h bl l Reachable Reachable workspace ( workspace (RW RW): reachable in at least ): reachable in at least
one orientation one orientation
How to maximize the dexterous workspace?
DW is a subset of RW f
the set of all possible positions of the end-effector:
the total volume swept out by the end-effector
as the manipulator executes all possible motions
Two Two- -link manipulator link manipulator
2 1
l l =
RWa disk of radius
2l
RW a disk of radius
DW the origin
1
2l
2 1
l l
RW
a ring of outer radius
2 1
l l +
g f
inner radius
2 1
2 1
l l
When joint limits are a subset of the full 360 degrees then the workspace When joint limits are a subset of the full 360 degrees, then the workspace
is correspondingly reduced.
Three Three--link manipulator: Multiple link manipulator: Multiple Three Three link manipulator: Multiple link manipulator: Multiple
solutions solutions
S lf i Th b b d i h Self-motion - The robot can be moved without
moving the end-effector from the goal
A second possible configuration in
which the same end-effector position
and orientation are achieved.
Possible Problems Possible Problems
Multiple solutions
Infinitely many solutions
No solutions No solutions
No closed-form (analytical) solutions
This only works if the number of kinematic constraints is This only works if the number of kinematic constraints is
the same as the number of degrees-of-freedom of the robot.
Multiple solutions Multiple solutions
We need to able to calculate all the possible
solutions.
The system has to be able to choose one The system has to be able to choose one.
The closest solution: the solution which
h h h d minimizes the amount that each joint is required
to move.
The closest solution in joint space The closest solution in joint space
Weights might be applied: moving small joints Weights might be applied: moving small joints
((wrist wrist) instead of moving large joints ( ) instead of moving large joints (Shoulder/ Shoulder/
Elbow Elbow))))
The presence of obstacles The presence of obstacles
Two possible solutions Two possible solutions
Eight solutions of the PUMA 560 Eight solutions of the PUMA 560
A h l i
180
o
+ =

Another solution
,
, 180
5 5
4 4
=

+ =


Left Arm Elbow Down Right Arm Elbow Down
. 180
6 6
5 5
o
+ =

Left Arm Elbow Up Right Arm Elbow Up
Number of solutions vs. nonzero Number of solutions vs. nonzero aa
i
A 6R ip l t
Li k l th
0
5 3 1
= = = a a a 4
A 6R manipulator
Link length
0
0
5 3
5 3 1
= = a a
a a a
8
4

0
3
5 3
= a 16
0
i
a All
16
The more the link length parameters are nonzero,
the bigger the maximum number of solutions!
Method of solution Method of solution
Closed form solutions: based on analytic
expressions or on the solution of a polynomial
of degree 4 or less Algebraic/geometric g g g
Numerical solutions: all systems with revolute
and prismatic joints having a total of 6 degrees and prismatic joints having a total of 6 degrees
of freedom in a single chain are solvable.
Much slower
Closed Closed- -form solutions form solutions
Analytical solution to system of equations
Can be solved in a fixed number of operations
(therefore, computationally fast/known speed) ( p y p )
Results in all possible solutions to the
manipulator kinematics manipulator kinematics
Often difficult or impossible to find
M d i bl f l i l Most desirable for real-time control
Most desirable overall
6R Manipulator: Three neighboring joint axes intersect at a point.
Robot 6DOF
Single Series Chain
d Revolute and Prismatic Joints
Analytic Solution Numeric Solution
Non
Real-Time
Real-Time
Real Time
(Iterative Solution)
Closed Form Solution
Sufficient Condition
Industrial
Three adjacent axes
(revolute or prismatic)
must intersect
d s
Robots
T T
0 0
) ( = T T
N N N 2 1
) , , , ( = K
A given desired pose of the tool (numbers!) g p f ( )
A function of the joint variables (equation!): found by
solving the forward kinematics
Numerical solutions Numerical solutions
Results in a numerical, iterative solution to system of
i f l N /R h h i equations, for example Newton/Raphson techniques
Unknown number of operations to solve
Only returns a single solution
Accuracy is dictated by user Accuracy is dictated by user
Because of these reasons, this is much less desirable
than a closed form solution than a closed-form solution
Can be applied to all robots.
Algebraic solution Algebraic solution (Fig. 4.7)
i
d

1 0 0 0
i
1 i

1 i
a
i
d
i

1 0 0 0
1

2 0 0 0
2

1
L
3 0 0 0
3

2
L

0 0
0 0 0
1 1
1 1
1 0 0 0 1 0 1
0 1 1
0




c s
s c
d s s c c c s
a s c

=
1 0 0 0
0 1 0 0
0 0
1 0 0 0
1 1
1 0 0 0 1 0 1
1 0 0 0 1 0 1 0
1


c s
d c c s c s s
d s s c c c s
T

0 0
1 0 0 0 1 0 0 0
2 3 3 1 2 2
L s c L s c

=
0 1 0 0
0 0
0 1 0 0
0 0
3 3 2
3
2 2 1
2
c s
T
c s
T

1 0 0 0 1 0 0 0
T T T T T
B
W
2
3
1
2
0
1
0
3
= =
0 . 0
12 2 1 1 123 123

+ c l c l s c
.
0 0 0 1 0 0 0 0
0 . 0
12 2 1 1 123 123
0
3

+
= =
s l s l c s
T T
B
W
1 0 0 0
0 . 0 0 . 1 0 . 0 0 . 0

0 . 0
0 . 0

y c s
x s c
B


.
1 0 0 0
0 . 0 0 . 1 0 . 0 0 . 0
0 . 0

=
y c s
T
B
W

2 1 2 1 12
2 1 2 1 12
s c c s s
s s c c c
+ =
=
1 0 0 0

2 1 2 1 12
123
123
s s
c c
=
=

A set of 4 nonlinear equation that must be

l d f
12 2 1 1
123
c l c l x
s s
+ =

solved for
3 2 1
, ,
2
2 2 2 2
12 2 1 1
c l l l l y x
s l s l y
+ + = +
+ =
2
2
2
2
1
2 2
2 2 1 2 1
l l y x
c
c l l l l y x
+
=
+ + = +
) ( 2 1
2
2
2 1
2
A
l l
c =
). , ( 2 tan 1
2 2 2
2
2 2
c s A c s = =
Elbow-up or elbow-down p
, ,
1 2 1 1 1 2 1 1
= + =
+ = =
s l k c l l k
c k s k y s k c k x
. ,
2 2 2 2 2 1 1
= + = s l k c l l k
sin cos
), , ( 2 tan ,
1 2
2
2
2
1


= =
= + =
r k r k
k k A k k r
. sin , cos
2 1
= = r k r k
), cos( sin sin cos cos
1 1 1
+ = =
r
x
). sin( cos sin sin cos
1 1 1
+ = + =
r
y
), , ( 2 tan ) , ( 2 tan
1
= = + x y A
r
x
r
y
A
). , ( 2 tan ) , ( 2 tan
1 2 1
= k k A x y A
r r
. ) , ( 2 tan
3 2 1


= = + + c s A
Some Inverse Some Inverse--Kinematic Formulas Kinematic Formulas
Th i l i The single equation
a = sin
(1)
has two solutions, given by
( ). , 1 2 tan
2
a a A = ( )
Likewise, given
, cos b =
( )
(2)
there are two solutions:
( ). 1 , 2 tan
2
b b A =
If both (1) and (2) are given, then there is a unique solution ( ) ( ) g q
given by
) ( 2 tan b a A = ). , ( 2 tan b a A =
The transcendental equation
has two solutions
0 sin cos = + b a
has two solutions
and
) , ( 2 tan b a A =
and
) , ( 2 tan b a A =
The equation q
c b a = + sin cos
is also solved by
( ) ( ). , 2 tan ) , ( 2 tan
2 2 2
c c b a A a b A + =
The set of equations
c b a = , sin cos
q
d b a = + cos sin
also is solved by
). , ( 2 tan bd ac bc ad A + =
A Geometric Approach A Geometric Approach
For most manipulators, many of the are
i i
d a ,
zero, the are 0 or etc.
In these cases a geometric approach is the
i

, 2 /
In these cases, a geometric approach is the
simplest and most natural.
Th l d d h l The general idea is to decompose the spatial
geometry of the arm into several plane-geometry
problems: solving simple trigonometry problems
Geometric Solution: 3 Geometric Solution: 3- -link Planar link Planar
Law of cosines
2 2 2 2
) 180 cos( 2
2 2 1
2
2
2
1
2 2
+ = + l l l l y x
2 2 2 2
l l y x +
2 1
2 1
2
2 l l
l l y x
c
+
=
Pythagorean (or Pythagoras) theorem
Geometric Solution: 3 Geometric Solution: 3- -link Planar link Planar
) , ( 2 tan x y A =
Law of cosines
) , (y
aw o cos es
2
2
2
1
2 2
cos
l l y x + +

2 2
1
2 1
2
cos
y x l
y
+
=
=
1
= + +
3 2 1
Elbow manipulator Elbow manipulator
) , ( 2 tan
) , ( 2 tan
1
1
c c
c c
x y A
x y A
+ =
=

1 c c
Elbow Manipulator: Singular Elbow Manipulator: Singular Elbow Manipulator: Singular Elbow Manipulator: Singular
configuration configuration
0 = =
c c
y x
P
; z
The wrist center intersects
Hence, any value of leaves
fixed There are thus infinitely
c
P
;
0
z
1

c
P
fixed. There are thus infinitely
many solutions for when
intersects
1

c
P
.
0
z
0
Elbow manipulator: Shoulder offset Elbow manipulator: Shoulder offset
Th ill i l b l There will, in general, be only two
solutions for
.
1

These correspond to the so-called

left arm and right arm configurations. g g
1
) (
=

( )
( )
2 2
, 2 tan
) , ( 2 tan
d r d A
x y A
c c
=
=

( )
2 2 2
, 2 tan d y x d A
c c
+ =
( )
1
2 2
1
, 2 tan ) , ( 2 tan d r d A x y A
c c
+ =
+ =

Left arm configuration

1
) , ( 2 tan x y A
c c
+ =
=

( )
2 2
, 2 tan d r d A =
Right arm configuration
Homework #10 (1 pt.) Homework #10 (1 pt.) Due Jan. 13
Find the angles for the elbow manipulator given
3 2
,
.
1

Homework #11 (1 pt.) Homework #11 (1 pt.) Due Jan. 13

Solve the inverse position kinematics for a 3 DOF spherical
manipulator.
Algebraic solution by reduction to Algebraic solution by reduction to Algebraic solution by reduction to Algebraic solution by reduction to
polynomial polynomial pp
Tangent of the half angle substitution

Tangent of the half angle substitution

1
,
2
tan
2
u
u

=

2
,
1
1
cos
2
u
u
u
+

=
.
1
2
sin
2
u
u
+
=
Example 4.3 Example 4.3
sin cos = + c b a
) 1 ( 2 ) 1 (
2 2
+ = + u c bu u a
0 ) ( 2 ) (
) 1 ( 2 ) 1 (
2
= + +
+ = +
a c bu u c a
u c bu u a
2 2 2
+

=
c a
c a b b
u
. tan 2
2 2 2
1

+

=

c a
c a b b

+ c a
Sine and Cosine of a Sum/Difference Sine and Cosine of a Sum/Difference
B A B A B A sin sin cos cos ) cos( = +
B A B A B A sin cos cos sin ) sin( + = +
B A B A B A sin sin cos cos ) cos( + =
B A B A B A sin cos cos sin ) sin( =
A A A A A
A A A
2 2 2 2
sin 2 1 1 cos 2 sin cos 2 cos
cos sin 2 2 sin =
A A A A A sin 2 1 1 cos 2 sin cos 2 cos = = =
Tangent of a Double Angle Tangent of a Double Angle
A tan 2
A
A
A
2
tan 1
tan 2
) 2 tan(

=
A A
A A
A
A
A
2 2
i
cos sin 2
) 2 (
) 2 sin(
) 2 tan( = =
A
A A A
2
2 2
cos
sin cos ) 2 cos(
) (

Divide both parts of the fraction by

B A
B A
tan tan
) tan(

=
B A
B A
tan tan 1
) tan(
m

Sine and Cosine of a Half Angle Sine and Cosine of a Half Angle
) 2 cos( ) 2 sin( 2 sin =
1 ) 2 ( cos 2 ) 2 ( sin ) 2 ( cos cos
) 2 cos( ) 2 sin( 2 sin
2 2 2


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

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

=
2
cos 1
) 2 cos(

+
=
Tangent of a Half Angle Tangent of a Half Angle
cos 1 ) 2 sin(

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

= =

cos 1
cos 1
) cos 1 (
) cos 1 )( cos 1 (
) 2 tan(
2
2
+

=
+
+
=

i
cos 1
1
sin
1
sin
) (
2

= = =
sin cos 1 cos 1 + +
) 90 cos( ) 90 cos( ) sin( sin = + = =
o o


) 90 sin( ) 90 sin( ) cos( cos
) 90 cos( ) 90 cos( ) sin( sin
= + = =
= + = =
o o
cot ) 90 tan( = +
o


tan ) tan(
cot ) 90 tan(
=
+
tan ) 180 tan( =
o
Piepers solution when three axes Piepers solution when three axes Pieper s solution when three axes Pieper s solution when three axes
intersect ( intersect (e.g. e.g., , spherical wrists) spherical wrists)
A completely general robot with six degrees of
freedom does not have a closed form solution.
The technique involves decoupling the position The technique involves decoupling the position The technique involves decoupling the position The technique involves decoupling the position
and orientation problems. The position and orientation problems. The position
problem positions the rist enter hile the problem positions the rist enter hile the problem positions the wrist center, while the problem positions the wrist center, while the
orientation problem completes the desired orientation problem completes the desired
orientation. orientation.
When the last three axes intersect the origins of link frames {4} {5} When the last three axes intersect, the origins of link frames {4}, {5},
and {6}are all located at this point of intersection.

) (
) (
3 2
3 1
1 0 3 4
3
2 1 0 3 2 1 0 0

f
f
T T
s d
a
T T T
y
x
P T T T P

= =
1
) (
) (
1 1
3 3
3 2 1
2
0
1
3 4
3 4 2
3
1
2
0
1 4
3 2
3
1
2
0
1 4
0
f
f
T T
c d
T T T
z
y
P T T T P
ORG ORG

1 1 1

0
1
a s c

0
1 1 1 1
1
1 i i i i i i i
i i i
i
i
d
d s s c c c s
a s c
T

1 0 0 0
1 1 1 1 i i i i i i i
d c c s c s s
2 3 3 4 3 3 3 1
a s s d c a a f

+ +

2 3 3 2 4 3 2 3 4 3 2 3
2 3 3 2 4 3 2 3 4 3 2 3
3 4
3 4 2
3
3
2
c d c c d c s s d s s a
s d c s d c c s d s c a
c d
s d
T
f
f

+ +

=

1 1 1
2 3 3 2 4 3 2 3 4 3 2 3 3 4 3
c d c c d c s s d s s a c d f

+ +

2 1 1 1
a f s f c g
g s g c
+ =

,
1 2 3 1 2 1 2 1 1 2 2
1 2 2 1 2 1
3
2 1 1 1
4
0
d f f f
s d f s f c c f c s g
a f s f c g
g
g c g s
P
ORG
+ =
+ =

+
=
1
1 2 3 1 2 1 2 1 1 2 3
3
c d f c f s c f s s g
g
+ + + =

2
3
2
2
2
1
2 2 2
g g g z y x r + + = + + =
) ( 2 2
2 2 1 2 1 3 2
2
2
2
1
2
3
2
2
2
1
f s f c a f d d a f f f + + + + + + =
) (
, 2 ) (
3 1 2 2 2 1
k s c k s k z
k a s k c k r
+ =
+ + = Dependence on has been eliminated
Dependence on takes a simple form
2

,
, ) (
1 1
4 1 2 2 2 1

f k
f k
k s c k s k z
=
+ =
p p f
2
3
g
, 2
,
3 2
2
2
2
1
2
3
2
2
2
1 3
2 2
f d d a f f f k
f k
+ + + + + =
=
3
.
1 2 1 3 4
c d c f k + =
Three Cases:
3 1
0 k r a = =
Three Cases:
0 k
4 1
0 k z s = =
Otherwise, eliminate and
2
s
2
c
We can solve for . , ,
3 2 1
can compute
.
0
0
4
=
R
0 1 0 4
Given
R R R
0
6
0
1 0
4
0
4
6
4 4
=

=
=

Kinematic Decoupling Kinematic Decoupling
Decouple the inverse kinematics problem into
two simpler problems: inverse position
kinematics, inverse orientation kinematics ,
First finding the position of the intersection of
the rist es then finding the orient tion of the wrist axes, then finding the orientation of
the wrist
Kinematic Decoupling Kinematic Decoupling
P P
0
) (
A function of only the first three joint variables
c
P
des
des
R R
P P
6 1
0
6
6 1 6
) , , (
) , , (
=
=


K
K
f f y f j
c
The direction of w r t {0}
z
R d P P
0
0
0

+ =
P
7
d
The direction of w.r.t. {0}
6
z
des c des
R d P P
7
0
1
0

+ =
c
P
7
P
des des c
R d P P
7
0
1
0
0

=
7
T
x c
R R R R R R R R r d P
r d P
y
x
0 1 0 3 3 0
13 7 7
1
= = =

des des des

z
y
c
c
R R R R R R R R
r d P
r d P
z
y
3 3 7 7 3
33 7 7
23 7 7
, = = =

Example: PUMA 560 Example: PUMA 560 (Refer to the Example: PUMA 560 Example: PUMA 560 (Refer to the
textbook)
Repeatability and Accuracy Repeatability and Accuracy
Repeatability: how precisely a manipulator can
return to a taught point?
Accuracy: the precision with which a computed Accuracy: the precision with which a computed
point can be attained.