inverse manipulator kinematics -...
TRANSCRIPT
Inverse Manipulator Kinematics
Review
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Direct Versus Inverse Kinematics
Direct (Forward) Kinematics
Given: Joint angles and links geometry
Compute: Position and orientation of the end
effector relative to the base frame
Inverse Kinematics
Given: Position and orientation of the end
effector relative to the base frame
Compute: All possible sets of joint angles and
links geometry which could be
used to attain the given position and
orientation of the end effetor
TTf N
B
T
0)(
)()( 011 TfTf N
B
T
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Central Topic - Inverse Manipulator Kinematics -
Examples
• Geometric Solution - Concept
Decompose spatial geometry into several plane
geometry
Examples - RRR (3R) manipulators -
Geometric Solution
• Algebraic Solution - Concept
Examples - PUMA 560 - Algebraic Solution
1000
333231
232221
131211
10
1
0
z
y
x
N
NNprrr
prrr
prrr
TTT
Direct Kinematics Goal (Numeric values)
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability
• Existence of Solutions
• Multiple Solutions
• Method of solutions
– Close form solution
• Algebraic solution
• Geometric solution
– Numerical solutions
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - Existence of Solution
• For a solution to exist, must be in the workspace of the manipulator
• Workspace - Definitions
– Dexterous Workspace (DW): The subset of space in which the robot end
effector can reach all orientation.
– Reachable Workspace (RW): The subset of space in which the robot end
effector can reach in at least 1 orientation
• The Dexterous Workspace is a subset of the Reachable Workspace
TN
0
RWDW
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - Existence of Solution - Workspace - 2R
Example 1 - 21 LL
Reachable
Workspace
Dexterous
Workspace
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - Existence of Solution - Workspace - 2R
Example 2 - 21 LL
Reachable
Workspace
NO Dexterous
Workspace
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - Existence of Solution - Workspace - 3R
Example 3 - 21 LL
Reachable
Workspace
&
Dexterous
Workspace
End Effector
Rotation
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - Multiple Solutions
• Problem: The fact that a manipulator
has multiple multiple solutions may
cause problems because the system
has to be able to choose one
• Solution: Decision criteria
– The closest (geometrically) -
minimizing the amount that each
joint is required to move
• Note 1: input argument -
present position of the
manipulator
• Note 2: Joint Weight -
Moving small joints (wrist)
instead of moving large
joints (Shoulder & Elbow)
– Obstacles exist in the workspace
- avoiding collision
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - Multiple Solutions
• Multiple solutions are a common
problem that can occur when solving
inverse kinematics because the
system has to be able to chose one
• The number of solutions depends on
the number of joints in the
manipulator but is also a function of
the links parameters
• Example: The PUMA 560 can reach
certain goals with 8 different
(solutions) arm configurations
– Four solutions are depicted
– Four solutions are related to a
“flipped” wrist
iai id
i
o
o
180
180
66
55
44
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - Multiple Solutions - Number of Solutions
• Task Definition - Position the end
effector in a specific point in the
plane (2D)
• No. of DOF = No. of DOF of the task
Number of solution: 2
(elbow up/down)
• No. of DOF > No. of DOF of the task
Number of solution:
Self Motion - The robot can be
moved without moving the the end
effector from the goal
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - Methods of Solutions
• Solution (Inverse Kinematics)- A “solution” is the set of joint variables
associated with an end effector’s desired position and orientation.
• No general algorithms that lead to the solution of inverse kinematic equations.
• Solution Strategies
– Closed form Solutions - An analytic expression includes all solution sets.
• Algebraic Solution - Trigonometric (Nonlinear) equations
• Geometric Solution - Reduces the larger problem to a series of plane
geometry problems.
– Numerical Solutions - Iterative solutions will not be considered in this
course.
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability
Robot - 6 DOF
Single Series Chain Revolute & Prismatic Joints
Analytic Solution
Numeric Solution
Close Form Solution Sufficient Condition
Three adjacent axes
(rotary or prismatic)
must intersect
Industrial
Robots
Real-Time Non
Real-Time
Iterations
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 1-7
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 1
• Equation
• Solution (Unique)
[-1,1]bco
[-1,1]a sin
bs
a
),(2tan baA
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 2
• Equation
• Solution
– Two Solutions
– Singularity at the Boundary
21co
[-1,1]a sin
as
a
)1,(2tan 2aaA
1802
1
1a , 90 oWhen
21sin
[-1,1]b cos
b
b
),1(2tan 2 bbA
2
1
1b , ,1800 0 oWhen
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 3
• Equation
• Solution
– Two Solutions apart
– Singularity
b
a
s
bs
co
sin
0)(sin)a(co
),(2tan
),(2tan
baA
baA
1800
0b
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 4
• Equation
0,,
)(sin)a(co
cba
cbs
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 4
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 4
•
• Solution
– Two Solutions
– For a solution to exist
– No solution (outside of the workspace)
– One solution (singularity)
),(2tan ),(2tan 222 abAccbaA
0222 cba
0222 cba
0222 cba
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 5
• Equation
• Solution
bs
a
sinco
sinsin
negative is sin if ),(2tan
positive is sin if ),(2tan
baA
baA
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 6
• Equation
• Solution
– For an exiting solution (the determinant must be positive)
gfse
dcsa
sinco
sinco
),(2tan cgdfdeagA
0ceaf
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 7
• Transcendental equations are difficult to solve because they are a function of
• Making the following substitutions yields an expression in terms of a single
veritable , Using this substitutions, transcendental equation are converted
into polynomial equation
sc ,kscf ),(
u
2
2
2
1
2sin
1
1cos
2
tan
u
u
u
u
u
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 7
• Transcendental equation
• Substitute with the following equations
• yields
cbsac
2
2
2
1
2sin
1
1cos
u
u
u
u
sc ,
0)(2)(
)1(2)1(
2
22
acbuuca
ucbuua
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics -
Generalized Algebraic (Analytical) Solutions – Case 7
• Which is solved by the quadratic formula to be
• Note
– If u is complex there is no real solution to the original transcendental
equation
– If then
ca
cabbu
222
ca
cabb 2221tan2
0 ca o180
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Manipulator Kinematics
Geometric Approach
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 1/
• Given:
– Manipulator Geometry
– Goal Point Definition: The position of the wrist in space
• Problem:
What are the joint angles ( ) as a function of the goal (wrist position and
orientation)
ddd zyx ,,
321 ,,
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 2/
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 3/
• The planar geometry - top view of the robot
x0
y0
xd
yd
z2
z3
L3
L4
1
),(2tan1 dd xyA
22
1 dd yxr
x2
x3
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 4/
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 5/
• The planar geometry - side view of the robot:
• where
z0
L1+L2 zd
L3
L4
r2
r1
221
2222222
12 )(ˆˆ LLzyxzyxzrr ddddd
z
3
)(ˆ21 LLzz d
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 6/
• By Apply the law of cosines we get
• Rearranging gives
• and
• Solving for we get
• Where is defined above in terms of known parameters
)cos(2)180cos(2 343
2
4
2
3343
2
4
2
3
2
2 LLLLLLLLr
43
2
4
2
3
2
23
2
)(
LL
LLrc
2
33 1 cs
),1tan( 3
2
33 ccA
3
3cddd zyxLL and,,,4,3
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 8/
• Finally we need to solve for
• where
L3
L4
r2
r1
α
β
2
3
2
2
),ˆ(2tan 1rzA
22
1 dd yxr )(ˆ21 LLzz d
z
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 9/
• Based on the law of cosines we can solve for
)cos(2 32
2
3
2
2
2
4 LrLrL
32
2
4
2
3
2
2
2 Lr
LLrc
),1(2tan 2
ccA
),1(2tan)),((2tan 222
212 ccAyxLLzA ddd
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - 3D RRR (3R) -
Geometric Solution - 10/
• Summary
)
))((2
)(,
))((2
)(1(2tan
)),((2tan
2
321
22
2
4
2
3
2
21
222
2
321
22
2
4
2
3
2
21
22
22
212
LLLzyx
LLLLzyx
LLLzyx
LLLLzyxA
yxLLzA
ddd
ddd
ddd
ddd
ddd
),(2tan1 dd xyA
)
2
)()(,
2
)()(1tan(
43
2
4
2
3
2
21
222
43
2
4
2
3
2
21
22
3LL
LLLLzyx
LL
LLLLzyxA dddddd
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Manipulator Kinematics
Analytical Approach
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Central Topic - Inverse Manipulator Kinematics -
Examples
• Algebraic Solution - Concept
Examples - PUMA 560 - Algebraic Solution
1000
333231
232221
131211
10
1
0
z
y
x
N
NNprrr
prrr
prrr
TTT
Direct Kinematics Goal (Numeric values)
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Solvability - PUMA 560
Given : PUMA 560 - 6 DOF,
Solve:
Total Number of Equations: 12
Independent Equations: 3 - Rotation Matrix
3 - Position Vector
Type of Equations: Non-linear
61
1000
333231
232221
131211
5
6
4
5
3
4
2
3
1
2
0
1
0
6
z
y
x
prrr
prrr
prrr
TTTTTTT
T0
6
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Central Topic - Inverse Manipulator Kinematics -
Examples
• Geometric Solution - Concept
Decompose spatial geometry into several plane
geometry
Examples - Planar RRR (3R) manipulators -
Geometric Solution
• Algebraic Solution - Concept
Examples - PUMA 560 - Algebraic Solution
1000
333231
232221
131211
10
1
0
z
y
x
N
NNprrr
prrr
prrr
TTT
Direct Kinematics Goal (Numeric values)
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 1/
• Given:
– Direct Kinematics: The homogenous transformation from the base to the
wrist
– Goal Point Definition: The position and orientation of the wrist in space
TB
W
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 2/
• Problem:
What are the joint angles ( ) as a function of the wrist position and
orientation ( or when is given as numeric values)
61 T0
6
1000
)()()()()()(333231
232221
131211
6
5
65
4
54
3
43
2
32
1
21
0
1
0
6
z
y
x
prrr
prrr
prrr
TTTTTTT
Goal Direct Kinematics
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 3/
• Solution (General Technique): Multiplying each side of the direct kinematics
equation by a an inverse transformation matrix for separating out variables in
search of solvable equation
• Put the dependence on on the left hand side of the equation by multiplying
the direct kinematics eq. with gives
)()()()()()()]([)]([ 6
5
65
4
54
3
43
2
32
1
21
0
1
1
1
0
1
0
6
1
1
0
1 TTTTTTTTT
1
1
0
1 )]([ T1
I
)()()()()()()],,,([)],,,([ 6
5
65
4
54
3
43
2
32
1
21
0
1
1
4321
0
4
0
6
1
4321
0
4 TTTTTTTTT
)()()()()()()]([)],,([ 6
5
65
4
54
3
43
2
32
1
21
0
1
1
1
0
3
0
6
1
321
0
3 TTTTTTTTT
)()()()()()()],,,,([)],,,,([ 6
5
65
4
54
3
43
2
32
1
21
0
1
1
54321
0
5
0
6
1
54321
0
5 TTTTTTTTT
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 4/
• Put the dependence on on the left hand side of the equation by multiplying
the direct kinematics eq. with gives
)()()()()()()]([)]([ 6
5
65
4
54
3
43
2
32
1
21
0
1
1
1
0
1
0
6
1
1
0
1 TTTTTTTTT
1
1
0
1 )]([ T1
I
1000
0100
00
00
11
11
0
1
cs
sc
T
1000
][ 1 BORG
ATA
B
TA
BB
A
A
B
PRRTT
1000
0100
00
00
][11
11
10
1
1
0
cs
sc
TT
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 5/
Tprrr
prrr
prrr
cs
sc
z
y
x
1
6
333231
232221
131211
11
11
10001000
0100
00
00
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 6/
1000
00
0100
00
22
22
1
2
cs
sc
T
1000
100
00
0
3
33
233
2
3d
cs
asc
T
1000
00
100
0
44
4
344
3
4
cs
d
asc
T
1000
00
0100
00
55
55
4
5
cs
sc
T
1000
00
0100
00
66
66
5
6
cs
sc
T
1000
'
33
'
32
'
31
'
'
23
'
22
'
21
'
'
13
'
12
'
11
'
1
6
z
y
x
prrr
prrr
prrr
T
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 7/
• Solution for
• Equating the (2,4) elements from both sides of the equation we have
• To solve the equation of this form we make the trigonometric substitution
Tprrr
prrr
prrr
cs
sc
z
y
x
1
6
333231
232221
131211
11
11
10001000
0100
00
00
311 dpcps yx
sin
cos
y
x
p
p
1
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 8/
• Substituting with we obtain
• Using the difference of angles formula
),(2tan
22
yx
yx
ppA
pp
yx pp , ,
311
dcssc
3
1)sin(d
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 9/
• Based on
• and so
• The solution for may be written
• Note: we have found two possible solutions for corresponding to the +/- sign
1)(cos)(sin 1
2
1
2
2
2
31 1)cos(
d
2
2
331 1,2tan
ddA
2
2
331 1,2tan),(2tan
ddAppA xy
1
1
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 10/
• Solution for
• Equating the (1,4) element and (3,4) element
• We obtain
Tprrr
prrr
prrr
cs
sc
z
y
x
1
6
333231
232221
131211
11
11
10001000
0100
00
00
2223423311 casdcapspc yx
22234233 casdcapz
3
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 11/
• If we square the following equations and add the resulting equations
311 dpcps yx
2223423311 casdcapspc yx
22234233 sacdsapz
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 11/ (Continue)
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 11/ (Continue)
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 12/
• we obtain
• where
• Note that the dependence on has be removed. Moreover the eq. for is of
the same form as the eq. for and so may be solved by the same kind of
trigonometric substitution to yield a solution for
Ksdca 3433
2
2
4
2
3
2
3
2
2
222
2a
adaapppK
zyx
1 3
13
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 13/
• Note that the +/- sign leads to two different solution for
3
22
4
2
3433 ,2tan),(2tan KdaKAdaA
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 14/
• Solution for
• Equating the (1,4) element and (2,4) element we obtain
• These equations may be solved simultaneously for and resulting in
)()()()()()()]([)],,([ 6
5
65
4
54
3
43
2
32
1
21
0
1
1
1
0
3
0
6
1
321
0
3 TTTTTTTTT
1000
0
10001000
0 546465464654
456565
3546465464654
333231
232221
131211
311
3223231231
3223231231
ssccscsscccs
dcsscs
asccssccssccc
prrr
prrr
prrr
dcs
sacsssc
cascscc
z
y
x
33223231231 acapspcspcc zyx
43223231231 dsapcpsspsc zyx
23s23c
I2
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 14/ (Continue)
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 15/
• Since the denominator are equal and positive, we solve for the sum of and
as
• The equation computes four values of according to the four possible
combination of solutions for and
2
11
2
43211223
23)(
))(()(
yxz
yxz
pspcp
dsapspcpcaas
2
11
2
11323432
23)(
))(()(
yxz
yxz
pspcp
pspccaapdsac
2 3
)])(()(
),)(()[(2tan
11323432
4321122323
yxz
yxz
pspccaapdsa
dsapspcpcaaA
23
31
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 16/
• Then, four possible solutions for are computed as
• Solution for
• Equating the (1,3) and the (3,3) elements
• we get
3232
2
1000
0
10001000
0 546465464654
456565
3546465464654
333231
232221
131211
311
3223231231
3223231231
ssccscsscccs
dcsscs
asccssccssccc
prrr
prrr
prrr
dcs
sacsssc
cascscc
z
y
x
5433232312323113 scrscsrccr
5
1231134
s
crsrs
4
5
332323123231134
s
rscsrccrc
54123113 sscrsr
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 17/
• As long as we can solve for
• When the manipulator is in a singular configuration in which joint axes
4 and 6 line up and cause the same motion of the last link of the robot. In this
case all that can be solved for is the sum or difference of and . This
situation is detected by checking whether both arguments of Atan2 defining
are near zero. If so is chosen arbitrary (usually chosen to be equal to the
present value of joint 4), and is computed later.
05 s 4
),(2tan 332323123231131231134 rscsrccrcrsrA
05
4
5
6
4 6
4
6
4
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 18/
• Solution for
• Equating the (1,3) and the (3,3) elements we get
• We can solve for
)()()()()()()],,,([)],,,([ 6
5
65
4
54
3
43
2
32
1
21
0
1
1
4321
0
4
0
6
1
4321
0
4 TTTTTTTTT
1000
0
00
0
10001000
56565
66
56565
333231
232221
131211
43223231231
4343432423414231414231
4343432423414231414231
csscs
cs
ssccc
prrr
prrr
prrr
dsacsssc
sacdscassccscscsscc
casdccacsscccsssccc
z
y
x
5423334142312341423113 )()()( scsrscccsrsscccr
523332312323113 )()()( ccrssrscr
5
),(2tan 555 csA
5
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 19/
• Solution for
• Equating the (3,1) and the (1,1) elements we get
• We can solve for
)()()()()()()],,,,([)],,,,([ 6
5
65
4
54
3
43
2
32
1
21
0
1
1
54321
0
5
0
6
1
54321
0
5 TTTTTTTTT
6523152313123215231523111 )()()( scsssscrcrssscscr
654154231541542313142321541542315415423111 )()()( ccsccccssssscccrcsrsscsccscssccccr
1000
00
0100
00
10001000
66
66
333231
232221
131211
432523152312352315231
434343254154231541542314235415423154154231
434343254154231541542314235415423154154231
cs
sc
prrr
prrr
prrr
dsacssssccssscsc
sacdscaccccscsscsssccsssccsscsccscscc
casdccacsccccsssssccccssscsccscsscccc
z
y
x
6 ),(2tan 666 csA
6
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 21/
• Summary - Number of Solutions
• Four solution
• For each of the four solutions the wrist can be
flipped
22
4
2
3433 ,2tan),(2tan KdaKAdaA
2
2
331 1,2tan),(2tan
ddAppA xy
o
o
180
180
6
'
6
5
'
5
4
'
4
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Inverse Kinematics - PUMA 560 -
Algebraic Solution - 22/
• After all eight solutions have been computed, some or all of them may have to
be discarded because of joint limit violations.
• Of the remaining valid solutions, usually the one closest to the present
manipulator configuration is chosen.
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• High Level Introduction to the Solution
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• High Level Introduction to the Solution
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• When the last three axes of a 6 DOF robot intersect, the origins of link frame {4},
{5}, and {6} are all located at the point of intersection. This point is given in the
base coordinate system as
• From the general forward kinematics method for determining homogeneous
transforms using DH parameters, we know:
orgorg PTTTP 4
32
3
1
2
0
14
0
1000
0
1111
1111
1
1
iiiiiii
iiiiiii
iii
i
idccscss
dsscccs
asc
T
Ri
i
1
iorg
i P1
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• For i=4
• Using the fourth column and substituting for , we find
1000
0
4333434
4333434
344
3
4dccscss
dsscccs
asc
T
1
43
43
3
2
3
1
2
0
14
32
3
1
2
0
14
0
dc
ds
a
TTTPTTTP orgorg
orgP4
3
orgP4
3R3
4
1
)(
)(
)(
33
32
31
1
2
0
14
32
3
1
2
0
14
0
f
f
f
TTPTTTP orgorg
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• where
11
)(
)(
)(
43
43
3
2
3
33
32
31
dc
ds
a
Tf
f
f
11000
0
1
)(
)(
)(
43
43
3
3222323
3222323
233
33
32
31
dc
ds
a
dccscss
dsscccs
asc
f
f
f
23324323432333
23324323432332
23343331
cdccdcssdssaf
sdcsdccsdscaf
assdcaf
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Repeating the same process again
1
)(
)(
)(
33
32
31
1
2
0
14
32
3
1
2
0
14
0
f
f
f
TTPTTTP orgorg
1
)(
)(
)(
1
)(
)(
)(
33
32
31
1
2
23
22
21
f
f
f
Tg
g
g
1
)(
)(
)(
1000
0
1
)(
)(
)(
33
32
31
2111212
2112212
122
23
22
21
f
f
f
dccscss
dsscccs
asc
g
g
g
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Repeating the same process for the last time
123121211223
123121211222
1221221
cdfcfscfssg
sdfsfccfssg
afsfcg
1
)(
)(
)(
22
22
21
0
14
32
3
1
2
0
14
0
g
g
g
TPTTTP orgorg
1
)(
)(
)(
1000
0
23
22
21
1000101
1000101
011
4
0
g
g
g
dccscss
dsscccs
asc
P org
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Frame {0} - The frame attached to the base of the robot or link 0 called frame
{0} This frame does not move and for the problem of arm kinematics can be
considered as the reference frame.
• Assign {0} to match {1} when the first joint veritable is zero
1
)(
)(
)(
1000
0
23
22
21
1000101
1000101
011
4
0
g
g
g
dccscss
dsscccs
asc
P org
0 0 0101 ad
1
3
2111
2111
4
0
g
gcgs
gsgc
P org
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Through algebraic manipulation of these equations, we can solve for the desired
joint angles ( ).
• The first step is to square the magnitude of the distance from the frame {0}
origin to frame {4} origin.
• Using the previously define function for we have
321 ,,
2
3
2
2
2
1
2
4
02
4
02
4
02 gggPPPr orgzorgyorgx
ig
2212132
2
2
2
1
2
3
2
2
2
1
2 2 fsfcafddafffr
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Applying a substitution of temporary variables, we can write the magnitude
squared term along with the z-component of the {0} frame origin to the {4} frame
origin distance.
• These equations are useful because dependence on has been eliminated,
and dependence on takes a simple form
123121211234
0 cdfcfscfssgPZ orgz
2212132
2
2
2
1
2
3
2
2
2
1
2 2 fsfcafddafffr
412221
312221
2 2
ksckskZ
kaskckr
12134
32
2
2
2
1
2
3
2
2
2
13
22
11
2
cdcfk
fddafffk
fk
fk
1
2
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Consider 3 cases while solving for :
• Case 1 -
• Solution Methodology - Reduction to Ploynomial => Quadratic Equation
3
01 a
3
2 kr
32
2
2
2
1
2
3
2
2
2
13 2 fddafffk
23324323432333
23324323432332
23343331
cdccdcssdssaf
sdcsdccsdscaf
assdcaf
22
2
1
2sin
1
1cos
2tan
u
u
u
uu
412221
312221
2 2
ksckskZ
kaskckr
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Case 2 -
• Solution Methodology - Reduction to Ploynomial => Quadratic Equation
01 s
4kZ
12134 cdcfk
23324323432333 cdccdcssdssaf
22
2
1
2sin
1
1cos
2tan
u
u
u
uu
412221
312221
2 2
ksckskZ
kaskckr
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Case 3 (General case) : We can find through the following algebraic
manipulation:
• squaring both sides, we find
3
2221
1
4
2221
1
3
2
2
cksks
kZ
skcka
kr
2221
2
2
2
2
2
2
2
1
2
2221
2
1
4
2221
2
2
2
2
2
2
2
1
2
2221
2
1
3
2
2
22
sckkckskcksks
kZ
sckkskckskcka
kr
412221
312221
2 2
ksckskZ
kaskckr
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• Adding these two equations together and simplifying using the trigonometry
identity (Reduction to Ploynomial), we find a fourth order equation for
3
2
2
2
1
2
1
4
2
1
3
2
2kk
s
kZ
a
kr
23324323432333
23324323432332
23343331
cdccdcssdssaf
sdcsdccsdscaf
assdcaf
12134
32
2
2
2
1
2
3
2
2
2
13
22
11
2
cdcfk
fddafffk
fk
fk
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• With solved, substitute into to find
412221
312221
2 2
ksckskZ
kaskckr
3 2Zr ,2
23324323432333
23324323432332
23343331
cdccdcssdssaf
sdcsdccsdscaf
assdcaf
12134
32
2
2
2
1
2
3
2
2
2
13
22
11
2
cdcfk
fddafffk
fk
fk
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• With solved, substitute into to find
• Solve for using the reduction to polynomial method
32,
1
3
2111
2111
4
0
g
gcgs
gsgc
P org
orgP4
0
21114
0
21114
0
gcgsP
gsgcP
orgy
orgx
23324323432333
23324323432332
23343331
cdccdcssdssaf
sdcsdccsdscaf
assdcaf
1
123121211223
123121211222
1221221
cdfcfscfssg
sdfsfccfssg
afsfcg
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Pieper’s Solution - Three consecutive Axes Intersect
• To complete our solution we need to solve for
• Since the last three axes intersect these joint angle affect the orientation of only
the last link. We can compute them based only upon the rotation portion of the
specified goal
• - The orientation of link frame {4} relative to the base frame {0} when
• are the Euler angles applied to
654 ,,
R0
6
RRR 0
60
10
40
4
6 44
0
0
4 4R
04
654 ,, 0
4
6 4R
RRR 4
60
0
4
0
6 4
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Central Topic - Inverse Manipulator Kinematics -
Examples
• Algebraic Solution (closed form) - Piepers Method (Continue) - Last three
consecutive axes intersect at one point
• Consider a 3 DOF (wrist) non-planar robot whose axes all intersect at a point.
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Mapping - Rotated Frames - Z-Y-Z Euler Angles
Start with frame {4}.
• Rotate frame {4} about by an angle
• Rotate frame {4} about by an angle
• Rotate frame {4} about by an angle
Note - Each rotation is preformed about an axis of the moving reference frame
{4}, rather then a fixed reference.
BY4Z
Euler Angles
4Z
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Mapping - Rotated Frames - Z-Y-Z Euler Angles
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Mapping - Rotated Frames - Z-Y-Z Euler Angles
100
0
0
0
010
0
100
0
0
)()()(),,('''
cs
sc
cs
sc
cs
sc
RRRR ZYZZYZ
csscs
ssccscsscccs
sccssccssccc
R ZYZ ),,('''
0
4
6''' 4),,( RR ZYX
A
B
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Three consecutive Axes Intersect - wrist
Goal
Direct Kinematics
csscs
ssccscsscccs
sccssccssccc
rrr
rrr
rrr
333231
232221
131211
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Three consecutive Axes Intersect - wrist
• Solve for using element
• Using the Atan2 function, we find
2222
32
2
31 scsrr
cr 33
2
32
2
31 rrs
33
2
32
2
31 ,Atan2 rrr
333231 ,, rrr
cr
ssr
csr
33
32
31
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Three consecutive Axes Intersect - wrist
• Solve for using elements
ssr 23
scr 13
srsr /,/Atan2 1323
1323, rr
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Three consecutive Axes Intersect - wrist
• Solve for using elements
ssr 32
csr 31
3132, rr
srsr /,/Atan2 3132
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Three consecutive Axes Intersect - wrist
• Note: Two answers exist for angle which will result in two answers each for
angles and .
• If the solution degenerates
33
2
32
2
31 ,Atan2 rrr
srsr /,/Atan2 1323
srsr /,/Atan2 3132
0180,0 soo
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Three consecutive Axes Intersect - wrist
• We are left with for every case. This means we can’t solve for either,
just their sum.
csscs
ssccscsscccs
sccssccssccc
rrr
rrr
rrr
333231
232221
131211
o0
100
0)()(
0)()(
100
0
0
333231
232221
131211
cs
sc
ccsssccs
csscsscc
rrr
rrr
rrr
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Three consecutive Axes Intersect - wrist
• One possible convention is to choose
• The solution can be calculated to be
o0
0
0
csrr ,Atan2,Atan2 1112
180
0
csrr ,Atan2,Atan2 1112
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Three consecutive Axes Intersect - wrist
• For this example, the singular case results in the capability for self-rotation.
That is, the middle link can rotate while the end effector’s orientation never
changes.
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Gimbal Lock
Normal situation
The three gimbals are independent
Gimbal lock:
Two out of the three gimbals are in the
same plane, one degree of freedom is lost
http://youtu.be/zc8b2Jo7mno
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA
Gimbal Lock – Robotics
• In robotics, gimbal lock is commonly referred to as "wrist flip", due to the use of
a "triple-roll wrist" in robotic arms, where three axes of the wrist, controlling yaw,
pitch, and roll, all pass through a common point.
• An example of a wrist flip, also called a wrist singularity, is when the path
through which the robot is traveling causes the first and third axes of the robot's
wrist to line up. The second wrist axis then attempts to spin 180° in zero time to
maintain the orientation of the end effector. The result of a singularity can be
quite dramatic and can have adverse effects on the robot arm, the end effector,
and the process.
• The importance of non-singularities in robotics has led the American National
Standard for Industrial Robots and Robot Systems — Safety Requirements to
define it as "a condition caused by the collinear alignment of two or more robot
axes resulting in unpredictable robot motion and velocities".
Instructor: Jacob Rosen
Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA