Introduction to ROBOTICS Kinematics of Robot Manipulator PROF.UJWAL HARODE UJWALHARODE1.
Inverse Manipulator Kinematics (1/3) - UCLA | Bionics Lab
Transcript of Inverse Manipulator Kinematics (1/3) - UCLA | Bionics Lab
Inverse Manipulator Kinematics (1/3)
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Solvability
• Existence of Solutions
• Multiple Solutions
• Method of solutions
– Close form solution
• Algebraic solution
• Geometric solution
– Numerical solutions
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Solvability - Existence of Solution - Workspace - 2R
Example 1 - 21 LL
Reachable
Workspace
Dexterous
Workspace
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Solvability - Existence of Solution - Workspace - 2R
Example 2 - 21 LL
Reachable
Workspace
NO Dexterous
Workspace
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Solvability - Existence of Solution - Workspace - 3R
Example 3 - 21 LL
Reachable
Workspace
&
Dexterous
Workspace
End Effector
Rotation
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Iterations
Mathematical Equations
• Law of Sinus / Cosines - For a general triangle
• Sum of Angles
c
C
b
B
a
A sinsinsin
A
B
C
a c
b
Abccba cos2222
21211221sin csscs
21211221cos ssccc
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 1/12
03 L
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
1000
0100
0022
1022
1
2
cs
Lsc
T
1000
0100
0011
0011
0
1
cs
sc
T
1000
0
1111
1111
1
1
dccscss
dsscccs
asc
Tiiiiii
iiiiiii
iii
i
i
1000
0100
0033
2033
2
3
cs
Lsc
T
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 2/12
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 3/12
• Using trigonometric identities to simplify , the solution to the forward
kinematics is:
• were
1000
0100
221)122(1032132132132132132132131
221)122(10321321321321321321321321
2
3
1
2
0
1
0
3
LscLcLscccssccssscsscccscsssccs
LssLcLcccsssscscsccscscsssscccc
TTTTTB
W
TB
W
1000
0100
0
0
12211123123
12211123123
0
3
sLsLcs
cLcLsc
TTB
W
321123 cos c 321123 sin s
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 4/12
• Given:
– Direct Kinematics: The homogenous transformation from the base to the
wrist
– Goal Point Definition: For a planar manipulator, specifying the goal can be
accomplished by specifying three parameters: The position of the wrist in
space ( ) and the orientation of link 3 in the plane relative to the axis
( )
TB
W
yx,
X
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 5/12
• Problem:
What are the joint angles ( ) as a function of the wrist position and
orientation ( )
• Solution:
• The goal in terms of position and orientation of the wrist expressed in terms of
the homogeneous transformation is defined as follows
321 ,, ,, yx
1000
0100
0
0
12211123123
12211123123
0
3
sLsLcs
cLcLsc
TTB
W
1000
0100
0
0
ycs
xsc
TGoal
B
W
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 6/12
• A set of four nonlinear equations which must be solved for
• Solving for
• If we square and add them while making use of ;
we obtain
TTGoal
B
W
0
3
12211
12211
123
123
slsly
clclx
ss
cc
321 ,,
x y212112 ssccc 212112 csscs
221
2
2
2
1
22 2 cllllyx
2
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 6/12 (Continue)
12211
12211
slsly
clclx
212112 ssccc
212112 csscs
221
2
2
2
1
22 2 cllllyx
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 7/12
• Solving for we obtain
• Note: In order for a solution to exist, the right hand side must have a value
between -1 and 1. Physically if this constraints is not satisfied, then the goal
point is too far away for the manipulator to reach.
• Assuming the goal is in the workspace, and making use of
we write an expression for as
• Note: The chose of the sign corresponds to the multiple solutions in which we
can choose the “elbow-up” or the “elbow-down” solution
2c
21
2
2
2
1
22
22 ll
llyxc
12
2
2
2 sc2s
2
22 1 cs
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 8/12
• Finally, we compute using the two argument arctangent function
2
)2
,1(2tanc,sAtan221
2
2
2
1
222
2222ll
llyxcA
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 8/12
• Solving for
• For solving we rewrite the original nonlinear equations using a change of
variables as follows
• where
1
12211
12211
slsly
clclx
1211
1211
cksky
skckx
222
2211
slk
cllk
2k
1k
22cl
1
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 8/12 (continue)
• Geometrical Interpretation
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 9/12
• Changing the way in which we write the constants and
• Then 2k
1k
22cl
1k2k
),(2tan 12
2
2
2
1
kkA
kkr
sin
cos
2
1
rk
rk
r
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 10/12
• Base on the previous two transformations the equations can be rewritten as
• or
• or
11 sinsincoscos rrx
11 cossinsincos rry
11 sinsincoscos r
x
11 cossinsincos r
y
)cos( 1 r
x
)sin( 1 r
y
1211
1211
cksky
skckx
sin
cos
2
1
rk
rk
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 11/12
• Using the two argument arctangent we finally get a solution for
• Note:
(1) When a choice of a sign is made in the solution of ( ),
it will cause a sign change in thus affecting
(2) If then the solution becomes undefined - in this case is arbitrary
1
),(2tan),(2tan1 xyAr
x
r
yA
),(2tan),(2tan 121 kkAxyA
222
2211
slk
cllk
12
2k
0 yx 1
),(2tan 12 kkABy Definition
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
)2
,1(2tanc,sAtan221
2
2
2
1
222
2222ll
llyxcA
Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 12/12
• Solving for
• Base on the original equations
• We can solve for the sum of
• Note: It is typical with manipulators that have two or more links moving in a
plane that in the course of a solution, expressions for sum of joint angles arise
3
123
123
ss
cc
321 ,,
),(2tan321 csA
213
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Geometric Solution - 1/5
• Given:
– Manipulator Geometry
– Goal Point Definition: The position and orientation of the wrist in
space
• Problem:
What are the joint angles ( ) as a function of the goal (wrist position and
orientation)
yx,
321 ,,
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Geometric Solution - 2/5
• Solution:
• We can apply the law of cosines to solve
for
• Since
• We have
2
22 cos)180cos(
21
2
2
2
1
22
22 ll
llyxc
2r
)180cos(2 221
2
2
2
1
222 llllyxr
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Geometric Solution - 3/5
21
2
2
2
1
22
22 ll
llyxc
2
22 1 cs
),(2tan 222 csA
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Geometric Solution - 4/5
• Note : Condition - Should be checked by
the computational algorithm to verify
existence of solutions.
• Assuming that the solution exist it lies in
the range of
• The other possible solution may found by
symmetry to be
2r
22
21 yxll
oo 1800 2
2
'
2
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -
Geometric Solution - 5/5
• By definition
• Defining as a function of x,y
• Applying the law of cosine to find
• Note:
1
1
2
02
02
),(2tan xyA
22
1
2
2
2
1
22
2cos
yxl
llyx
o18000
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
)cos,cos1(2tan),(2tan 2
1 AxyA
Inverse Kinematics - Planar RRR (3R) -
Geometric Solution - 6/5
• Angle in the plane add up to define the
orientation of the last link
213
321
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Manipulator Kinematics (2/4)Inverse Manipulator Kinematics (2/4)
Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
Central Topic - Inverse Manipulator Kinematics -ExamplesExamples
• Geometric Solution - ConceptDecompose spatial geometry into several planegeometry
Examples - Planar RRR (3R) manipulators -G t i S l tiGeometric Solution
• Algebraic Solution - Concept
⎤⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
== −
1000333231
232221
131211
101
0
z
y
x
NNN prrr
prrrprrr
TTT K
Examples - PUMA 560 - Algebraic Solution
Direct Kinematics Goal (Numeric values)
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -Geometric Solution - 1/5Geometric Solution 1/5
• Given:– Manipulator Geometry– Goal Point Definition: The position and orientation of the wrist in
space• Problem:
What are the joint angles ( ) as a function of the goal (wrist position and
yx,
θθθ
φ
What are the joint angles ( ) as a function of the goal (wrist position and orientation)
321 ,, θθθ
φ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -Geometric Solution - 2/5Geometric Solution 2/5
• Solution:
• We can apply the law of cosines to solve for
2θ
)180cos(222222 θ+=+= llllyxr
• Since
cos)180cos( θθ −=− 2θr
)180cos(2 22121 θ−−+=+= llllyxr
• We have
22 cos)180cos( θθ =
21
22
21
22
2 2 llllyxc −−+
=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -Geometric Solution - 3/5Geometric Solution 3/5
22
21
22 llyxc −−+=
212 2 llc =
222 1 cs −±= 22
),(2tan 222 csA=θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -Geometric Solution - 4/5Geometric Solution 4/5
• Note : Condition - Should be checked by the computational algorithm to verify
i f l iexistence of solutions.
2221 yxll +≥+
• Assuming that the solution exist it lies in the range of
2θr
oo 1800 ≤≤θ
• The other possible solution may found by symmetry to be
oo 1800 2 ≤≤θ
symmetry to be
2'2 θθ −=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -Geometric Solution - 5/5Geometric Solution 5/5
• By definition
ψβθ ±=1
02 >θ
02 <θ
• Defining as a function of x,y2θ
2
β
)(2tan xyA=β
• Applying the law of cosine to find1θ
),(2tan xyA=β
22
21
22 llyx −++
N t
221
21
2cos
yxlllyx
+
++=ψ
o18000 ≤≤ψ• Note: 1800 ≤≤ψ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - Planar RRR (3R) -Geometric Solution - 6/5Geometric Solution 6/5
• Angle in the plane add up to define the orientation of the last link
321 θθθφ ++=
213 θθφθ +−=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 1/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
TBW
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 2/Algebraic Solution 2/
• Problem:What are the joint angles ( ) as a function of the wrist position and 61 θθ Lorientation ( or when is given as numeric values)
61T06
⎥⎤
⎢⎡ 131211 xprrr
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
==
1000
)()()()()()(333231
2322216
565
454
343
232
121
01
06
z
y
prrrprrr
TTTTTTT θθθθθθ
{GoalDirect Kinematics
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 3/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
h f l bl isearch of solvable equation
• Put the dependence on on the left hand side of the equation by multiplying the direct kinematics eq. with gives 1
101 )]([ −θT
1θq g
)()()()()()()]([)]([ 6565
454
343
232
121
01
11
01
06
11
01 θθθθθθθθ TTTTTTTTT −− =
11 )]([ θT
I)()()()()()()()( 66554433221111611
)()()()()()()]([)]([ 54321010010 θθθθθθθθθθθθθθ TTTTTTTTT −−
)()()()()()()]([)],,([ 6565
454
343
232
121
01
11
03
06
1321
03 θθθθθθθθθθ TTTTTTTTT −− =
)()()()()()()],,,([)],,,([ 6565
454
343
232
121
01
14321
04
06
14321
04 θθθθθθθθθθθθθθ TTTTTTTTT =
)()()()()()()],,,,([)],,,,([ 6565
454
343
232
121
01
154321
05
06
154321
05 θθθθθθθθθθθθθθθθ TTTTTTTTT −− =
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 4/Algebraic Solution 4/
• Put the dependence on on the left hand side of the equation by multiplying the direct kinematics eq. with gives 1
101 )]([ −θT
1θ
)()()()()()()]([)]([ 6565
454
343
232
121
01
11
01
06
11
01 θθθθθθθθ TTTTTTTTT −− ={
I
⎥⎥⎤
⎢⎢⎡ −
0000
11
11
0 θθθθ
cssc
T ⎥⎥⎤
⎢⎢⎡
−−][ 1 BORGATA
BTA
BBA PRRTT
⎥⎥⎥
⎦⎢⎢⎢
⎣
=
10000100
1101T
⎥⎥⎥
⎦⎢⎢⎢
⎣
==
1000
][ 1 BA
AB TT
⎤⎡ 0011 θθ sc
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
== −
100001000000
][ 11
11
101
10
θθθθcssc
TT
⎥⎦
⎢⎣ 1000
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 5/Algebraic Solution 5/
prrrsc x13121111 00 ⎤⎡⎤⎡
Tprrrprrrp
cs
z
y
x
16
333231
232221
131211
11
11
10001000010000
=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
10001000 ⎦⎣⎦⎣
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 6/Algebraic Solution 6/
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
−−
−
=00010000
22
22
12 θθ
θθ
cs
sc
T
⎥⎥
⎦⎢⎢
⎣ 100022
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡ −
=100
000
3
33
233
23 d
csasc
Tθθθθ
⎥⎦
⎢⎣ 1000
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−−
−
=00
1000
44
4
344
34 θθ
θθ
csdasc
T
⎥⎥⎥⎤
⎢⎢⎢⎡
= ''''
'23
'22
'21
'
'13
'12
'11
'
16
y
x
prrrprrrprrr
T⎥⎦
⎢⎣ 1000
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−
=
100000010000
55
55
45 θθ
θθ
cs
sc
T⎥⎥⎥
⎦⎢⎢⎢
⎣ 1000333231 zprrr
⎦⎣ 1000
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−−
−
=
100000010000
66
66
56 θθ
θθ
cs
sc
T
⎦⎣ 1000
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 7/Algebraic Solution 7/
Tprrrprrrprrr
cssc
z
y
x
16
333231
232221
131211
11
11
01000000
=⎥⎥⎥⎤
⎢⎢⎢⎡
⎥⎥⎥⎤
⎢⎢⎢⎡−
• Equating the (2 4) elements from both sides of the equation we have
pz333231
10001000000
⎥⎥
⎦⎢⎢
⎣⎥⎥
⎦⎢⎢
⎣
Equating the (2,4) elements from both sides of the equation we have
311 dpcps yx =+−
• To solve the equation of this form we make the trigonometric substitution
φρφρ
sincos
==x
pp
φρ sinyp
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 8/Algebraic Solution 8/
22 pp +=ρ
S b tit ti ith bt i
),(2tan yx
yx
ppA
pp
=
+=
φ
ρ
φ• Substituting with we obtainyx pp , φρ ,
ρφφ3
11dcssc =−
• Using the difference of angles formula
ρ
ρθφ 3
1)sin( d=−
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 9/Algebraic Solution 9/
• Based on 1)(cos)(sin 12
12 =−+− θφθφ
• and so2
23
1 1)cos(ρ
θφ d−±=−
⎟⎟⎠
⎞⎜⎜⎝
⎛−±=− 2
233
1 1,2tanρρ
θφ ddA
• The solution for may be written
⎟⎞
⎜⎛
±233 12t)(2tθ ddAA
1θ
• Note: we have found two possible solutions for corresponding to the +/- sign
⎟⎟⎠
⎜⎜⎝
−±−= 233
1 1,2tan),(2tanρρ
θ AppA xy
1θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 10/Algebraic Solution 10/
• Equating the (1,4) element and (3,4) element
prrrprrr
cssc
y
x
1232221
131211
11
11
0000
⎥⎥⎤
⎢⎢⎡
⎥⎥⎤
⎢⎢⎡−
Tprrrpcs
z
y 16
333231
23222111
10001000010000
=
⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣
• We obtain2223423311 casdcapspc yx +−=+
casdcap ++=− 22234233 casdcapz ++=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 11/Algebraic Solution 11/
• If we square the following equations and add the resulting equations
dpcps =+− 311 dpcps yx =+
2223423311 casdcapspc yx +−=+
22234233 sacdsapz ++=−
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 11/ (Continue)Algebraic Solution 11/ (Continue)
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 11/ (Continue)Algebraic Solution 11/ (Continue)
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 12/Algebraic Solution 12/
• we obtain
Kd
• where
Ksdca =− 3433
2222222
2
24
23
23
22
222
2aadaappp
K zyx −−−−++=
• 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
1θ 3θ1θ
3θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 13/Algebraic Solution 13/
• Note that the +/- sign leads to two different solution for 3θ
( )224
23433 ,2tan),(2tan KdaKAdaA −+±−=θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 14/Algebraic Solution 14/
)()()()()()()]([)]([ 54321010010 θθθθθθθθθθ TTTTTTTTT −− =I
)()()()()()()]([)],,([ 6655443322111363213 θθθθθθθθθθ TTTTTTTTT =
⎤⎡ −−−−⎤⎡⎤⎡ −− asccssccsscccprrrcascscc
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−−−−−
=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−−−−−
10000
100010000 546465464654
456565
3546465464654
333231
232221
131211
311
3223231231
3223231231
ssccscsscccsdcsscsasccssccssccc
prrrprrrprrr
dcssacsssccascscc
z
y
x
• Equating the (1,4) element and (2,4) element we obtain⎦⎣⎦⎣⎦⎣
33223231231 acapspcspcc zyx =−−+
• These equations may be solved simultaneously for and resulting in
43223231231 dsapcpsspsc zyx =+−−−
23s 23c
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 14/ (Continue)Algebraic Solution 14/ (Continue)
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 15/Algebraic Solution 15/
43211223 ))(()( yxz dsapspcpcaas
−++−−= 2
11223 )( yxz pspcp
s++
=
2211323432
23 )())(()( yxz
pspcppspccaapdsa
c++
+−−−−=
• Since the denominator are equal and positive, we solve for the sum of and as
11 )( yxz pspcp ++
2θ 3θ
)])(()(),)(()[(2tan
11323432
4321122323
yxz
yxz
pspccaapdsadsapspcpcaaA
+−−−−
−++−−=θ
• The equation computes four values of according to the four possible combination of solutions for and
23θ3θ1θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 16/Algebraic Solution 16/
• Then, four possible solutions for are computed as
θθθ2θ
• Equating the (1,3) and the (3,3) elements
3232 θθθ −=
⎥⎥⎤
⎢⎢⎡
−−−−−−
⎥⎥⎤
⎢⎢⎡
⎥⎥⎤
⎢⎢⎡
−−−−−
456565
3546465464654
232221
131211
3223231231
3223231231
dcsscsasccssccssccc
prrrprrr
sacsssccascscc
y
x
• we get
⎥⎥⎥
⎦⎢⎢⎢
⎣
−−−=
⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣
−−10000
100010000 546465464654333231311 ssccscsscccsprrrdcs z
y
5433232312323113 scrscsrccr −=−+
54123113 sscrsr =+−
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 17/Algebraic Solution 17/
• As long as we can solve for05 ≠s 4θ
)(2tAθ
• 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
),(2tan 332323123231131231134 rscsrccrcrsrA +−−+−=θ05 =θ
θ θ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 are near zero. If so is chosen arbitrary (usually chosen to be equal to the present value of joint 4), and is computed later, it will be computed accordingly
4θ 6θ
4θ6θ
5
6
4
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 18/Algebraic Solution 18/
)()()()()()()],,,([)],,,([ 6565
454
343
232
121
01
14321
04
06
14321
04 θθθθθθθθθθθθθθ TTTTTTTTT −− =
⎥⎤
⎢⎡ −−
⎥⎤
⎢⎡⎥⎤
⎢⎡ −+−−−+ 0565651312114343432423414231414231 sscccprrrcasdccacsscccsssccc x
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−=
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−−−−+−−+−
1000000
1000100056565
66
333231
232221
43223231231
4343432423414231414231
csscscs
prrrprrr
dsacssscsacdscassccscscsscc
z
y
• Equating the (1,3) and the (3,3) elements we get
5423334142312341423113 )()()( scsrscccsrsscccr −=−−++
• We can solve for523332312323113 )()()( ccrssrscr =−+−+−
5θ)(2tan csA=θ ),(2tan 555 csAθ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 19/Algebraic Solution 19/
)()()()()()()],,,,([)],,,,([ 6565
454
343
232
121
01
154321
05
06
154321
05 θθθθθθθθθθθθθθθθ TTTTTTTTT −− =
=
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
−−−−+−−+−−+−−+++−−+−−+++−+
333231
232221
131211
432523152312352315231
434343254154231541542314235415423154154231
434343254154231541542314235415423154154231
prrrprrrprrr
dsacssssccssscscsacdscaccccscsscsssccsssccsscsccscscccasdccacsccccsssssccccssscsccscsscccc
z
y
x
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
−−
−
⎥⎦
⎢⎣⎥⎦
⎢⎣
00010000
10001000
66
66
333231432523152312352315231
cs
sc
pz
• Equating the (3,1) and the (1,1) elements we get
6523152313123215231523111 )()()( scsssscrcrssscscr =++++−
⎥⎥
⎦⎢⎢
⎣ 100066
• We can solve for
6523152313123215231523111 )()()(
654154231541542313142321541542315415423111 )()()( ccsccccssssscccrcsrsscsccscssccccr =−+++++−+
6θ ),(2tan 666 csA=θ ),(2tan 666 csAθ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 21/Algebraic Solution 21/
• Summary - Number of Solutions
• Four solution
⎟⎟⎠
⎞⎜⎜⎝
⎛−±−= 2
233
1 1,2tan),(2tanρρ
θ ddAppA xy
( )224
23433 ,2tan),(2tan KdaKAdaA −+±−=θ
⎟⎠
⎜⎝ ρρ
• For each of the four solutions the wrist can be flipped
o1804'4 +=θθ
o1806'6
5'5
+=
−=
θθ
θθ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - PUMA 560 -Algebraic Solution - 22/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 chosenmanipulator configuration is chosen.
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Manipulator Kinematics (3/4)Inverse Manipulator Kinematics (3/4)
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Central Topic - Inverse Manipulator Kinematics -ExamplesExamples
• Geometric Solution - ConceptDecompose spatial geometry into severalplane geometry
Example - 3D - RRR (3R) manipulators -G t i S l tiGeometric Solution
• Algebraic Solution (closed form) -Algebraic Solution (closed form) Piepers Method - Last three consecutive axes intersect at one point
Example - Puma 560
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 1/Geometric Solution 1/
• Given:– Manipulator Geometry– Goal Point Definition: The position of the wrist in spaceddd zyx ,,
• Problem:What are the joint angles ( ) as a function of the goal (wrist position and orientation)
321 ,, θθθ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 2/Geometric Solution 2/
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 3/Geometric Solution 3/
• The planar geometry - top view of the robot
ydL4
x3
x0
y0
xd
2
z3L3
1θx2
z2
),(2tan1 dd xyA=θ
221 dd yxr +=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 4/Geometric Solution 4/
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 5/Geometric Solution 5/
• The planar geometry - side view of the robot:
L3L4
r2
3θ−
L1+L2 zd
r1z
z0
( )22222222 )(ˆˆ LLyxyxrr ++++++
• where
( )2112 )( LLzyxzyxzrr ddddd +−++=++=+=
)(ˆ 21 LLzz d +−=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 6/Geometric Solution 6/
• By Apply the law of cosines we get
• Rearranging gives
)cos(2)180cos(2 34324
23343
24
23
22 θθ LLLLLLLLr ++=+−+=
222
• and 43
24
23
22
3 2)(
LLLLrc +−
=
21
• Solving for we get
233 1 cs −=
2
3θ
• Where is defined above in terms of known parameters
),1tan( 3233 ccA −±=θ
3c ddd zyxLL and,,,4,3
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 8/Geometric Solution 8/
L3L4
r2α
β
2θ
3θ−
z
• Finally we need to solve for
r12
2θ
βαθ +=2
),ˆ(2tan 1rzA=α
• where 221 dd yxr += )(ˆ 21 LLzz d +−=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 9/Geometric Solution 9/
• Based on the law of cosines we can solve for β
)cos(2 3223
22
24 βLrLrL −+=
24
23
22 LLrc −+
32
432
2 Lrc =β
),1(2tan 2βββ ccA −±= ββ
),1(2tan)),((2tan 222212 ββθ ccAyxLLzA ddd −±+++−= ),()),(( 212 ββyddd
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics - 3D RRR (3R) -Geometric Solution - 10/Geometric Solution 10/
• Summary
22
),(2tan1 dd xyA=θ
( ) ( ) )))((2
)(,))((2
)(1(2tan
)),((2tan
2321
22
24
23
221
222
2321
22
24
23
221
22
22212
LLLzyxLLLLzyx
LLLzyxLLLLzyxA
yxLLzA
ddd
ddd
ddd
ddd
ddd
+−++
−++−++⎟⎟
⎠
⎞
⎜⎜
⎝
⎛
+−++
−++−++−±
+++−=θ
))((2))((2 321321 LLLzyxLLLzyx dddddd +++⎠⎝ +++
( ) ( ) ))()(,)()(1tan(24
23
221
22224
23
221
22
3LLLLzyxLLLLzyxA dddddd +−+−++
⎥⎤
⎢⎡ +−+−++
−±=θ )2
,2
1tan(4343
3 LLLLA ⎥
⎦⎢⎣
±θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -G li d Al b i (A l ti l) S l ti C 1 7Generalized Algebraic (Analytical) Solutions – Case 1-7
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -Generalized Algebraic (Analytical) Solutions – Case 1Generalized Algebraic (Analytical) Solutions Case 1
• Equation
[ 1 1]bco [-1,1]a sin
∈=∈=
bsa
θθ
S l ti (U i )
[-1,1]bco ∈= bsθ
• Solution (Unique) ),(2tan baA=θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -Generalized Algebraic (Analytical) Solutions – Case 2Generalized Algebraic (Analytical) Solutions Case 2
• Equation
21
[-1,1]a sin a
±
∈=
θ
θ21i
[-1,1]a cos
b
b ∈=
θ
θ
S l ti
21co as −±=θ 21sin b−±=θ
• Solution
– Two Solutions
)1,(2tan 2aaA −±=θ
1 θθ =
),1(2tan 2 bbA −±=θ
1 θθ =
– Singularity at the Boundary
1802 θθ −= 2 θθ −=
1a , 90 =±= oWhen θ 1b , ,1800 0 == oWhen θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -Generalized Algebraic (Analytical) Solutions – Case 3Generalized Algebraic (Analytical) Solutions Case 3
• Equation
abs =+
θθθ
sin0)(sin)a(co
• SolutionT S l ti t
ba
s−=
θθ
cosin
1800– Two Solutions apart
),(2tan ),(2tanbaAbaA
−=−=
θθ
1800
– Singularity
),(
0=b
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -Generalized Algebraic (Analytical) Solutions – Case 4Generalized Algebraic (Analytical) Solutions Case 4
• Equation )(sin)a(co =+ cbs θθ
• SolutionT S l ti
0,, ≠cba
– Two Solutions
),(2tan ),(2tan 222 abAccbaA +−+±=θ
– For a solution to exist
0222 >−+ cba
– No solution (outside of the workspace)
– One solution (singularity)
0222 <−+ cba
0222 =−+ cba
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -Generalized Algebraic (Analytical) Solutions – Case 4Generalized Algebraic (Analytical) Solutions Case 4
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -Generalized Algebraic (Analytical) Solutions – Case 4Generalized Algebraic (Analytical) Solutions Case 4
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -Generalized Algebraic (Analytical) Solutions – Case 5Generalized Algebraic (Analytical) Solutions Case 5
• Equation
bsa
==
φθφθ
sinco sinsin
• Solution
bs =φθ sinco
positiveissinif ),(2tan φθ baA=negative is sin if ),(2tanp)(
φθφ
baA −−=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Kinematics -Generalized Algebraic (Analytical) Solutions – Case 6Generalized Algebraic (Analytical) Solutions Case 6
• Equation
gfsedcsa
=+=+
θθθθ
sinco sinco
S l ti
gfse =+ θθ sinco
)(2 dfdAθ• Solution
– For an exiting solution (the determinant must be positive)
),(2tan cgdfdeagA −−=θ
0>− ceaf
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Algebraic Solution by Reduction to Polynomial
• Transcendental equations are difficult to solve because they are a function of
θθ sc
• Making the following substitutions yields an expression in terms of a single veritable Using this substitutions transcendental equation are converted
θθ sc ,kscf =),( θθ
uveritable , Using this substitutions, transcendental equation are converted into polynomial equation
u
θ
2
2
11cos
2
tan
uu
u
+−
=
=
θ
θ
212sin
1
uuu
+=
+
θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Algebraic Solution by Reduction to Polynomial - Example
• Transcendental equation
cbsac =+ θθ
• Substitute with the following equations
cbsac =+ θθ
θθ sc ,
2
2
2i
11cos
uuu
+−
=
θ
θ
• yields
21sin
u+=θ
)1(2)1( 22 +=+− ucbuua0)(2)(
)()(2 =−+−+ acbuuca
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Algebraic Solution by Reduction to Polynomial - Example
• Which is solved by the quadratic formula to be
cacabbu
+−−±
=222
⎟⎟⎠
⎞⎜⎜⎝
⎛
+−−±
= −
cacabb 222
1tan2θ
• Note
– If u is complex there is no real solution to the original transcendental– If u is complex there is no real solution to the original transcendental equation
– If then 0=+ ca o180=θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Solvability
Robot - 6 DOF Single Series Chain
Revolute & Prismatic Joints
Analytic Solution Numeric Solution Real-Time NonReal-Time
Close Form SolutionSufficient Condition Industrial Three adjacent axes (rotary or prismatic)
must intersect
IndustrialRobots
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• Pieper’s Solution - Closed form solution for a serial 6 DOF in which three consecutive axes intersect at a point (including robots with three consecutive
ll l i h i i fi i )parallel axes, since they meet at a point at infinity)
• Pieper’s method applies to the majority of commercially available industrial robots
• Example: (Puma 560)
– All 6 joints are revolute joints– The last 3 joints are intersecting
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• Given:
– Manipulator Geometry: 6 DOF & DH parameters• All 6 joints are revolute joints• The last 3 joints are intersecting
– Goal Point Definition: The position and orientation of the wrist in space
⎥⎤
⎢⎡ 131211 xprrr
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
==
1000333231
23222156
45
34
23
12
01
06
z
y
prrrprrr
TTTTTTT
• Problem:What are the joint angles ( ) as a function of the goal (wrist position and orientation)
,,,,, 654321 θθθθθθ
⎦⎣ 1000
position and orientation)
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 b dibase coordinate system as
orgorg PTTTP 432
312
014
0 =
• From the general forward kinematics method for determining homogeneous transforms using DH parameters, we know:
⎥⎤
⎢⎡ − −0 1iii asc θθ
Rii1−
iorgi P1−
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−−=
−−−−
−−−−−
10001111
11111
iiiiiii
iiiiiiiii dccscss
dsscccsT
αααθαθαααθαθ
⎦⎣ 000
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• For i=4orgP4
3R34
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
−−−
=
0
4333434
4333434
344
34 dccscss
dsscccsasc
Tαααθαθαααθαθ
θθ
• Using the fourth column and substituting for we find
⎥⎦
⎢⎣ 1000
4333434
P3Using the fourth column and substituting for , we find
⎥⎥⎤
⎢⎢⎡− 43
3
21032100 dsa
TTTPTTTPα
orgP4
⎥⎥⎤
⎢⎢⎡
)()(
32
31
1032100 θθ
ff
TTPTTTP
⎥⎥⎥
⎦⎢⎢⎢
⎣
==
143
4323
12
014
323
12
014
0
dcTTTPTTTP orgorg α
⎥⎥⎥
⎦⎢⎢⎢
⎣
==
1)()(
33
3212
014
323
12
014
0
θff
TTPTTTP orgorg
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• where
⎥⎥⎤
⎢⎢⎡
⎥⎥⎤
⎢⎢⎡
)()( 331
dsa
ff
αθθ
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−=
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣ 11)()(
43
4323
33
32
dcds
Tff
αα
θθ
⎥⎥⎥⎤
⎢⎢⎢⎡−
⎥⎥⎥⎤
⎢⎢⎢⎡
−−−
=⎥⎥⎥⎤
⎢⎢⎢⎡ 0
)()()(
43
43
3
3222323
3222323
233
33
32
31
dcds
a
dccscssdsscccs
asc
fff
αα
αααθαθαααθαθ
θθ
θθθ
⎥⎥
⎦⎢⎢
⎣⎥⎥
⎦⎢⎢
⎣⎥⎥
⎦⎢⎢
⎣ 110001)( 43322232333 dcdccscssf ααααθαθθ
( ) 23343331 αθ assdcaf ++=( )( )( ) 23324323432333
23324323432332
23343331
ααααααθααααααθcdccdcssdssafsdcsdccsdscaf
f
++−=−−−=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• Repeating the same process again
⎥⎤
⎢⎡
)()( 31
θθ
ff
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
==
1)()(
33
3212
014
323
12
014
0
θθ
ff
TTPTTTP orgorg
⎥⎥⎥⎤
⎢⎢⎢⎡
=⎥⎥⎥⎤
⎢⎢⎢⎡
)()()(
)()()(
33
32
31
12
23
22
21
θθθ
θθθ
fff
Tggg
⎥⎥
⎦⎢⎢
⎣⎥⎥
⎦⎢⎢
⎣ 1)(
1)( 3323 θθ fg
⎥⎤
⎢⎡⎥⎤
⎢⎡ −
⎥⎤
⎢⎡ )(0)( 3112221 θθθθ fascg
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−−=
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣ 1)()(
10001)()(
33
32
2111212
2112212
23
22
θθ
αααθαθαααθαθ
θθ
ff
dccscssdsscccs
gg
⎦⎣⎦⎣⎦⎣
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
( )( ) 123121211222
1221221
ααααθθ
sdfsfccfssgafsfcg
−−+=++=
• Repeating the same process for the last time
( )( ) 123121211223
123121211222
ααααθ cdfcfscfssgfffg+++=
⎥⎥⎥⎤
⎢⎢⎢⎡
==)()()(
22
22
21
014
323
12
014
0
θθθ
ggg
TPTTTP orgorg
⎥⎥
⎦⎢⎢
⎣ 1)( 22 θg
⎥⎤
⎢⎡⎥⎤
⎢⎡ − )(0 21011 θθθ gasc
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−−=
1)()(
100023
22
1000101
10001014
0
θθ
αααθαθαααθαθ
gg
dccscssdsscccs
P org
⎦⎣⎦⎣
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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
id d h f fconsidered as the reference frame.
• Assign {0} to match {1} when the first joint veritable is zero
00 dθ
⎥⎥⎤
⎢⎢⎡
⎥⎥⎤
⎢⎢⎡
−−−
)()(0
22
21
1000101
011
0 θθ
αααθαθθθ
gg
dsscccsasc
P
0 0 0101 ===≠ adαθ
⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣
=
1)(
100023
22
1000101
10001014
0
θαααθαθ gdccscssP org
⎤⎡ − 2111 gsgc
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡+
=
13
2111
2111
40
ggcgsgsgc
P org
⎥⎦
⎢⎣ 1
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• Through algebraic manipulation of these equations, we can solve for the desired joint angles ( ).
321 ,, θθθ
• The first step is to square the magnitude of the distance from the frame {0} origin to frame {4} origin.
( ) ( ) ( ) 23
22
21
24
024
024
02 gggPPPr orgzorgyorgx ++=++=
• Using the previously define function for we have ig
( )221213222
21
23
22
21
2 2 fsfcafddafffr −++++++= ( )221213221321 2 fsfcafddafffr ++++++
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
( )221213222
21
23
22
21
2 2 fsfcafddafffr −++++++=
• 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
1231212112340 αααα cdfcfscfssgPZ orgz +++===
squared term along with the z-component of the {0} frame origin to the {4} frame origin distance.
( ) 3122212 2 kaskckr ++=
22
11
fkfk−=
=
Th ti f l b d d h b li i t d
( ) 412221 ksckskZ +−= α
12134
3222
21
23
22
213 2
αα cdcfkfddafffk
+=+++++=
θ• These equations are useful because dependence on has been eliminated, and dependence on takes a simple form
1θ2θ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• Consider 3 cases while solving for :3θ
• Case 1 - 01 =a
32 kr =
3222
21
23
22
213 2 fddafffk +++++=
( ) 23343331 αθ assdcaf ++=( )( ) 23324323432333
23324323432332
ααααααθααααααθcdccdcssdssafsdcsdccsdscaf
++−=−−−=
• Solution Methodology - Reduction to Ploynomial => Quadratic Equation
22
2
12sin
11cos
2tan
uu
uuu
+=
+−
== θθθ112 uu ++
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• Case 2 - 01 =αs
kZ 4kZ =
12134 αα cdcfk +=
( )θ dddf
• Solution Methodology - Reduction to Ploynomial => Quadratic Equation
( ) 23324323432333 ααααααθ cdccdcssdssaf ++−=
22
2
12sin
11cos
2tan
uu
uuu
+=
+−
== θθθ
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• Case 3 (General case) : We can find through the following algebraic manipulation:
3θ
( )22211
32
2kZ
skckakr
+=−
• squaring both sides, we find
( )22211
4 ckskskZ
−=−α
q g ,
( ) 222122
22
22
21
22221
2
32
22
sckkskckskckakr
++=+=⎟⎟⎠
⎞⎜⎜⎝
⎛ −
( ) 222122
22
22
21
22221
2
1
4
1
2
2
sckkckskckskskZ
a
−+=−=⎟⎟⎠
⎞⎜⎜⎝
⎛ −
⎟⎠
⎜⎝
α ⎠⎝
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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θ
22
21
2
1
4
2
1
32
2kk
skZ
akr
+=⎟⎟⎠
⎞⎜⎜⎝
⎛ −+⎟⎟
⎠
⎞⎜⎜⎝
⎛ −α
22
11
fkfk−=
=
( )θ df ++
12134
3222
21
23
22
213 2
αα cdcfkfddafffk
+=+++++=
( )( )( ) 23324323432333
23324323432332
23343331
ααααααθααααααθ
αθ
cdccdcssdssafsdcsdccsdscaf
assdcaf
++−=−−−=
++=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• With solved, substitute into to find
( )2
3θ 2θZr ,2
( )( ) 412221
3122212 2
ksckskZkaskckr
+−=++=
α
2222222
11
2 fddfffk
fkfk−=
=
( ) 23343331 αθ assdcaf ++=
12134
3222
21
23
22
213 2
αα cdcfkfddafffk
+=+++++=
( )( )( ) 23324323432333
23324323432332
23343331
ααααααθααααααθ
αθ
cdccdcssdssafsdcsdccsdscaf
assdcaf
++−=−−−=
++
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• With solved, substitute into to find32,θθ
⎤⎡
orgP40
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
+−
=3
2111
2111
40
ggcgsgsgc
P org
⎥⎦
⎢⎣ 1
3g
211140 gsgcP orgx −=
211140 gcgsP orgy
g
+=
( )( )
23343331 αθ assdcaf ++=( )( )
1221221 θ afsfcg ++=
• Solve for using the reduction to polynomial method
( )( ) 23324323432333
23324323432332
ααααααθααααααθcdccdcssdssafsdcsdccsdscaf
++−=−−−=
1θ
( )( ) 123121211223
123121211222
ααααθααααθcdfcfscfssgsdfsfccfssg
+++=−−+=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Pieper’s Solution - Three consecutive Axes Intersect
• To complete our solution we need to solve for 654 ,, θθθ
• 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 R06
RRR 060
1040
46
44 =−
= = θθ
RRR 460
04
06 4 =
= θ
• - The orientation of link frame {4} relative to the base frame {0} when 004 4 =θR
04 =θ
• are the Euler angles applied to 654 ,, θθθ 046 4 =θR
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Inverse Manipulator Kinematics (4/4)Inverse Manipulator Kinematics (4/4)
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Central Topic - Inverse Manipulator Kinematics -ExamplesExamples
• 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 Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Mapping - Rotated Frames - Z-Y-Z Euler Angles
Start with frame {4}. • Rotate frame {4} about by an angle 4Z α}• 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} th th fi d f
γBY4
β} Euler Angles
4Z
{4}, rather then a fixed reference.
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Mapping - Rotated Frames - X-Y-Z Euler Angles
⎤⎡⎤⎡⎤⎡ 000 γγββαα scscsc
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡ −
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡ −==
10000
0010
0
10000
)()()(),,(''' γγγγ
ββ
ββαααα
γβαγβα cssc
cs
sccssc
RRRR ZYZZYZ
⎥⎥⎤
⎢⎢⎡
+−+−−−
= βαγαγβαγαγβαβαγαγβαγαγβα
γβα ssccscsscccssccssccssccc
R ZYZ ),,('''
⎥⎥⎦⎢
⎢⎣ − βγβγβ
βγγβγγβγβcsscs
ZYZ ),,(
046''' 4
),,( == θγβα RR ZYXAB
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Three consecutive Axes Intersect - wrist
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡+−+−−−
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
βγβγββαγαγβαγαγβαβαγαγβαγαγβαssccscsscccssccssccssccc
rrrrrr
232221
131211
{Di t Ki ti
⎥⎦⎢⎣ −⎥⎦⎢⎣ βγβγβ csscsrrr 333231
Goal Direct Kinematics
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Three consecutive Axes Intersect - wrist
• Solve for using element β 333231 ,, rrr
( )222232
231 ααβ scsrr +=+
βcr =33 βcr33
232
231 rrs +±=β
• Using the Atan2 function, we find
( )22( )332
322
31 ,Atan2 rrr +±=β
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Three consecutive Axes Intersect - wrist
• Solve for using elementsα 1323, rr
βαssr =23
βαscr =13
β23
( )ββα srsr /,/Atan2 1323=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Three consecutive Axes Intersect - wrist
• Solve for using elementsγ3132 , rr
γβssr =32
γβcsr −=31
( )ββγ srsr /,/Atan2 3132 −=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Three consecutive Axes Intersect - wrist
• Note: Two answers exist for angle which will result in two answers each for angles and .
βα γ
( )332
322
31 ,Atan2 rrr +±=β
( )ββα srsr /,/Atan2 1323=
( )ββγ srsr //Atan2 3132 −=
• If the solution degenerates
( )ββγ srsr /,/Atan2 3132=
0180,0 =⇒== βββ soo
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Three consecutive Axes Intersect - wrist
⎤⎡⎤⎡ βαγαγβαγαγβα sccssccsscccrrr
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−+−+−−−
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
βγβγββαγαγβαγαγβαβαγαγβαγαγβα
csscsssccscsscccssccssccssccc
rrrrrrrrr
333231
232221
131211
o0=β
⎤⎡⎤⎡⎤⎡ 0)()(0
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡+++−+
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡+−+−−−
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
1000)()(0)()(
10000
333231
232221
131211
γαγαγαγα
γαγαγαγαγαγαγαγα
cssc
ccsssccscsscsscc
rrrrrrrrr
• We are left with for every case. This means we can’t solve for either, just their difference.
( )αγ +
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
Three consecutive Axes Intersect - wrist
• One possible convention is to choose o0=α
• The solution can be calculated to be
0=β
0=α
180=β
0=α
( ) ( )γγγ csrr ,Atan2,Atan2 1112 =−= ( ) ( )γγγ csrr ,Atan2,Atan2 1112 =−=
Instructor: Jacob Rosen Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 Ph.D.Models of Robot Manipulation – CE 215 - Department of Computer Engineering
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 263B - 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 263B - Department of Mechanical & Aerospace Engineering - UCLA