L5 - Inverse Kinematics V1
54
Universiti Kuala Lumpur Malaysia France Institute Inverse Kinematics Solution FAB30703 Originally prepared by: Prof Engr Dr Ishkandar Baharin Head of Campus & Dean UniKL MFI
-
Upload
zul-fadhli -
Category
Documents
-
view
188 -
download
12
Transcript of L5 - Inverse Kinematics V1
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Inverse Kinematics Solution
FAB30703
Originally prepared by: Prof Engr Dr Ishkandar BaharinHead of Campus & Dean
UniKL MFI
Forward Kinematics Solution (FKS) vs. Inverse Kinematics Solution (IKS)
In FKS we built a tool for finding end frame geometry from Given Joint data:
In IKS we need Joint models from given End Point Geometry:
Joint Space
Cartesian Space
Joint Space Cartesian
Space
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
So this IKS Problem is Nasty (as we already know!)
It is a more difficult problem because:The Equation set is “Over-Specified”:
12 equations in 6 unknowns for a 6 DOF robot
Space can be “Under-Specified”:Planer devices with more joints than 2
The Solution set can contain Redundancies: Multiple solutions
The Solution Sets may be un-defined:Unreachable in 1 or many joints
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
But the IKS is VERY Useful –some Uses of IKS include:
Building Workspace Map
Allow “Off-Line Programming” solutions
The IKS allows the engineer to equate Workspace capabilities with Programming realities to assure that execution is feasible
The IKS Aids in Workplace Design and Operational Simulations
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
A Simple Example for a
X
Y
r
Revolute and Prismatic Joints Combined
(x , y)
Finding
More Specifically:
arctan2( ) specifies that it’s in the first quadrant
Finding r:
( )2 2r x y= +
rθ − robot
1θ
1 arctan yx
θ ⎛ ⎞= ⎜ ⎟⎝ ⎠
1 arctan 2 yx
θ ⎛ ⎞= ⎜ ⎟⎝ ⎠
1θ
Given World Space Position (x, y), find the Joint Space Solution rθ −
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
(x , y)
l2
l1
Inverse Kinematics of a Two Link Manipulator
Given: l1, l2 , x , y in World Space
Find:
Redundancy:A unique solution to this problem
does not exist. Notice, that using the “givens” two solutions are possible. Sometimes no solution is possible.
(x , y)l2
l1l2
l1
1θ 2θ
1θ
2θ in Joint Space
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
The Geometric Solution
l1
l2
α
(x , y) Using the Law of Cosines:
⎟⎟⎠
⎞⎜⎜⎝
⎛ −−+=
−−+=
−=−−−+=+
−+=
21
22
21
22
21
22
21
22
212
22
122
222
2arccosθ
2)cos(θ
)cos(θ)θ180cos()θ180cos(2)(
cos2
llllyx
llllyx
llllyx
Cabbac
2
2
22
2
Using the Law of Cosines:
1 2 22 2 2 2
2
1 1
1 1
sin sin
sinθ sin(180 θ ) sin(θ )x y x y
θ θ αθ θ α
yα arctan 2x
B Cb c
l
=
−= =
+ +
= −
= +
⎛ ⎞= ⎜ ⎟⎝ ⎠
⎟⎠⎞
⎜⎝⎛+
⎟⎟
⎠
⎞
⎜⎜
⎝
⎛
+=
xy2arctan
yx)sin(θarcsinθ
2222
1l
Redundant since θ2 could be in the first or fourth quadrant.
Redundancy caused since θ2 has two possible values
1θ
2θ
1θ
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
( ) ( )( )
⎟⎟⎠
⎞⎜⎜⎝
⎛ −−+=∴
++=
+++=
+++++=
=+=+
++
++++
21
22
21
22
2
2212
22
1
211211212
22
1
211212
212
22
12
1211212
212
22
12
1
2222
2yxarccosθ
c2
)(sins)(cc2
)(sins2)(sins)(cc2)(cc
yx)2((1)
llll
llll
llll
llllllll
The Algebraic Solution
l1
l2Υ(x , y)
1 1
1 2 1 2
1 1 2 1 2
1 1 2 1 2
1 2
c cosθc cos(θ θ )(1 ) x c c(2 ) y s sin(3 ) θ θ θ
l ll l
+
+
+
== += += += +
Only Unknown
))(sin(cos))(sin(cos)sin(
))(sin(sin))(cos(cos)cos(:
abbaba
bababaNote
+−
+−
−+
+−
=
=
1θ
2θ
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
))(sin(cos))(sin(cos)sin(
))(sin(sin))(cos(cos)cos(:
abbaba
bababaNote
+−
+−
−+
+−
=
=
)c(s)s(c cscss
sinsy
)()c(c ccc
ccx
2211221
12221211
21211
2212211
21221211
21211
llllll
ll
slsllsslll
ll
++=++=
+=
−+=−+=
+=
+
+
We know what θ2 is from the previous slide. We need to solve for θ1 . Now we have two equations and two unknowns (sin θ1 and cos θ1 )
( )
2222221
1
2212
22
1122221
221122221
221
221
2211
yxx)c(ys
)c2(sx)c(
1
)c(s)s()c()(xy
)c()(xc
+
−+=
++++
=
+++
+=
++
=
slll
llllslll
lllll
slsll
sls
Substituting for c1 and simplifying many times
Notice this is the law of cosines and can be replaced by x2+ y2
⎟⎟⎠
⎞⎜⎜⎝
⎛
+
−+= 22
222211 yx
x)c(yarcsinθ slllUni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Doing a Pure IKS solution: the θR Manipulator
θθR Frame Skeleton (as DH R Frame Skeleton (as DH Suggest!)Suggest!)
X0
Y0
Z0
Z1
X1
Y1 Y2
X2
Z2
Same Point!
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
LP Table and Ai’s
1
1 0 1 01 0 1 0
0 1 0 00 0 0 1
S CC S
A
−⎡ ⎤⎢ ⎥⎢ ⎥=⎢ ⎥⎢ ⎥⎣ ⎦
010100d2 + cl2
0P21 2
-S1C1019000θ + 90
R10 1
C θS θCαSααadθVarLinkFrames
22 2
1 0 0 00 1 0 00 0 10 0 0 1
Ad cl
⎡ ⎤⎢ ⎥⎢ ⎥=⎢ ⎥+⎢ ⎥⎣ ⎦
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
FKS is A1*A2:
2 2
1 0 1 0 1 0 0 01 0 1 0 0 1 0 0
0 1 0 0 0 0 10 0 0 1 0 0 0 1
S CC S
d cl
−⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥• =⎢ ⎥ ⎢ ⎥+⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦
2 2
2 2
1 0 1 1 ( )1 0 1 1 ( )
0 1 0 00 0 0 1
S C C d clC S S d cl− +⎡ ⎤⎢ ⎥+⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦U
nive
rsiti
Kua
la L
umpu
r Mal
aysi
a Fr
ance
Inst
itute
Forming The IKS:
2 2
2 2
1 0 1 1 ( )1 0 1 1 ( )
0 1 0 00 0 0 1
S C C d clC S S d cl− +⎡ ⎤⎢ ⎥+⎢ ⎥ =⎢ ⎥⎢ ⎥⎣ ⎦ 0 0 0 1
x x x x
y y y y
z z z z
n o a dn o a dn o a d
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
In the Inverse Problem, The RHS MATRIX is completely known (perhaps from a robot mapped
solution)! And we use these values to find a solution to the joint equation that populate the LHS MATRIX
o
a
n
d
0x 0y
0z
KNOWN
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te , , , ,
0 1n o a x y zO D
F⎡ ⎤
= ⎢ ⎥⎣ ⎦
F
Forming The IKS:Examining these two matrices
n, o, a and d are given in the inverse sense!!!(But typically we want to build general models leaving these terms unspecified)
Term (1, 4) & (2,4) on both sides allow us to find an equation for θ:
(1,4): C1*(d2+cl2) = dx
(2,4): S1*(d2+cl2) = dy
Form a ratio to build Tan(θ): S1/C1 = dy/ dx
Tan θ = dy/dx
θ = Atan2(dx, dy)
If 2 Matrices are Equal then EACH and EVERY term is also Uniquely equal as well!
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Forming The IKS:
After θ is found, back substitute and solve for d2:
(1,4): C1*(d2+cl2) = dx
Isolating d2: d2 = [dx/Cosθ1] - cl2
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Alternative Method – “doing a pure inverse approach”
Form A1-1 then pre-multiply both side by
this ‘inverse’Leads to: A2 = A1
-1*T0ngiven
2 2
1 0 0 0 1 1 0 00 1 0 0 0 0 1 00 0 1 1 1 0 00 0 0 1 0 0 0 1 0 0 0 1
x x x x
y y y y
z z z z
S C n o a dn o a d
d cl C S n o a d
−⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥= •⎢ ⎥ ⎢ ⎥ ⎢ ⎥+⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Solving:
Selecting and Equating (1,4)0 = -S1*dx + C1*dy
Solving: S1*dx = C1*dy
Tan(θ) = (S1/C1) = (dy/dx)θ = Atan2(dx, dy) – the same as before
Selecting and Equating (3,4) -- after back substituting θ solution
d2 + cl2 = C1*dx + S1*dy
d2 = C1*dx + S1*dy - cl2
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Performing IKS For Industrial Robots: (a more involved problem)
First lets consider the concept of the Spherical Wrist Simplification
All Wrist joint Z’s intersect at a pointThe n Frame is offset from this Z’s intersection point at a distance dn (the hand span) along the avector of the desired solution (3rd column of desired orientation sub-matrix)This follows the DH Algorithm design tools as we have learned them!
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Performing IKS – with a Spherical Wrist
We can now separate the POSE effects:ARM joints
Joints 1 to 3 in a full function manipulator (without redundant joints)They function to maneuver the spherical wrist to a target POSITION related to the desired target POSE
WRIST JointsJoints 4 to 6 in a full functioning spherical wristWrist Joints function as a primary tool to ORIENTthe end frame as required by the desired target POSE
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Performing IKS: Focus on Positioning
We will define a point (called the WRIST CENTER) as:
Pc = [Px, Py, Pz]We find that this position is exactly:
Pc = dtarget - dn*aPx = dtarget,x - dn*ax
Py = dtarget,y - dn*ay
Pz = dtarget,z - dn*az
Note: dn is the so called ‘Handspan’ (a CONSTANT)
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Focusing on the ARM Manipulatorsin terms of Pc:
Prismatic:q1 = d1= Pz (its along Z0!) – cl1q2 = d2 = Px or Py - cl2q3 = d3= Py or Px - cl3
Cylindrical:θ1 = Atan2(Px, Py)d2 = Pz – cl2d3 = Px/C1 – cl3 {or +(Px
2 + Py2).5 – cl3}
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Focusing on the ARM Manipulatorsin terms of Pc:
Spherical:θ1 = Atan2(Px, Py)θ2 = Atan2( (Px
2 + Py2).5 , Pz)
D3 = (Px2 + Py
2 + Pz2).5 – cl3
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Focusing on the ARM Manipulatorsin terms of Pc:
Articulating:θ1 = Atan2(Px, Py)θ3 = Atan2(D, ±(1 – D2).5)
Where D =
θ2 = φ - αφ is: Atan2((Px2 + Py2).5, Pz)
α is:
( )2 2 2 2 22 3
2 32x y zP P P a a
a a+ + − −
( )( )( )
22 3
2 2 2 2 22 3
sintan 2( )cos
2 1tan 2
x y z
A
a a DA
P P P a a
αα =
⎛ ⎞± −⎜ ⎟⎜ ⎟+ + + −⎜ ⎟⎝ ⎠U
nive
rsiti
Kua
la L
umpu
r Mal
aysi
a Fr
ance
Inst
itute
Focusing on the ARM Manipulatorsin terms of Pc:
θ2 =
Where D =
( )( )( )
22 3
2 2 2 2 22 3
2 1tan 2
x y z
a a DA
P P P a a
⎛ ⎞± −⎜ ⎟⎜ ⎟+ + + −⎜ ⎟⎝ ⎠
( )2 2 2 2 22 3
2 32x y zP P P a a
a a+ + − −
Atan2((Px2 + Py2).5, Pz) -
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
One Further Complication Must Be Considered:
This is called the d2 offset problemA d2 offset is a problem that states that the nth frame has a non-zero offset along the Y0axis as observed in the solution of the T0
n
with all joints at home This leads to two solutions for θ1 the So-Called “Shoulder Left” and “Shoulder Right” solutions
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Defining the d2 Offset issue
X0, X1
Y0, Z1
Z0 d2
The ARMThe Wrist
Ypc
Xpc
Zpc
Here: ‘The ARM’ might contain a prismatic joint (as in the Stanford Arm –discussed in text) or it might be the a2 & a3 links in an Articulating Arm as it rotates out of plane
A d2 offset means that there are two places where θ1 can be placed to touch a given point (and note, when θ1 is at Home, the wrist center is not on the X0 axis!)
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Lets look at this Device “From the Top” – a plan view of the structure projected to the X0 Y0 plane
Pc'(Px, Py)
X0
Y0
Z1
Z1'
X1
d2
d2
a2'
a3'
R'
X1'
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
1φ
1α1θ 1X R′=
Solving For θ1:
We will have a Choice of two poses for θ1:
( )( )
1 1 1
1
.52 2 22 2
11 tan 2( , )
tan 2 ,
pc pc
pc pc
A X Y
A X Y d d
θ φ αθ
= −=
− + −
( )( )( )
2 1 1
.52 2 22 2
1 180
180 tan 2 ,
tan 2 ,
pc pc
pc pc
A X Y d d
A X Y
θ α φ= ° + + =
° + + − +
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
It is called a “Hard Arm” Solution
We have two θ1’s
These lead to two θ2’s (Spherical)One for Shoulder Right & one for Shoulder Left
Or four θ2’s and θ3’s in the Articulating Arm:
Shoulder Right Elbow Up & DownShoulder Left Elbow Up & Down
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Now, lets look at Orientation IKS’s
Orientation IKS again relies on separation of joint effectsWe (now) know the first 3 joints control positions
they would have been solved by using the appropriate set of equations developed above
The last three (wrist joints) will control the achievement of our desired Orientation
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
These Ideas Lead to an Orientation Model for a Device that is Given by:
This ‘model’ separates Arm Joint and Wrist Joint Contribution to the desired Target Orientation (note target orientation is a given for the IKS model!)
3 60 3 givenR R R=
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Focusing on Orientation Issues
Lets begin by considering ‘Euler Angles’ (they are a model that is almostidentical to a full functioning Spherical Wrist as defined using the D-H algorithm!):Step 1, Form a Product:
Rz1*Ry2*Rz3
This product becomes R36 in
the model on the previous slide
1
2
3
cos sin 0sin cos 0
0 0 1
cos 0 sin0 1 0
sin 0 cos
cos sin 0sin cos 0
0 0 1
z
y
z
R
R
R
φ φφ φ
θ θ
θ θ
ψ ψψ ψ
−⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥−⎣ ⎦
−⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Euler Wrist Simplified:C C C S S C C S S C C SS C C C S S C S C C S S
S C S S C
φ θ ψ φ ψ φ θ ψ φ ψ φ θφ θ ψ φ ψ φ θ ψ φ ψ φ θ
θ ψ θ ψ θ
− − −⎡ ⎤⎢ ⎥+ − +⎢ ⎥⎢ ⎥−⎣ ⎦
this matrix, which contains the joint control angles, is then set equal to a ‘U matrix’ prepared by multiplying the inverse of the ARM joint orientation sub-matrices and the Desired (given) target orientation sub-matrix:
( ) 130
x x x
y y y
z z z given
n o aR n o a
n o a
−⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
NOTE: R03 is
Manipulator dependent!
Inverse requires its TransposeU
nive
rsiti
Kua
la L
umpu
r Mal
aysi
a Fr
ance
Inst
itute
Simplifying the U Matrix: (a RHS matrix relative to the R3
6 Euler Product)
( )
30
130
11 12 1321 22 2331 32 33
11 21 3112 22 3213 23 33
(this is a transpose!)
R R RR R R R
R R R
R R RR R R R
R R R
−
⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
∴
⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Continuing: to define the U-Matrix
11 12 1321 22 2331 32 33
11 21 31 11 21 31 11 21 3112 22 32 12 22 32 12 22 3213 23 33 13 23 33 13 23 33
x y z x y z x y z
x y z x y z x y z
x y z x y z x y z
U U UU U UU U U
n R n R n R o R o R o R a R a R a Rn R n R n R o R o R o R a R a R a Rn R n R n R o R o R o R a R a R a R
⎡ ⎤⎢ ⎥ =⎢ ⎥⎢ ⎥⎣ ⎦⎡ ⎤+ + + + + +⎢ ⎥+ + + + + +⎢ ⎥⎢ + + + + + +⎣ ⎦⎥
Rij are terms from the product of the first 3 Ai’s
ni, oi & ai are from given target orientation
– we develop our models in a general way to allow computation of specific angles for specific cases
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Now Set LHS (Euler angles) = RHS (U-Matrix):
C C C S S C C S S C C SS C C C S S C S C C S S
S C S S C
φ θ ψ φ ψ φ θ ψ φ ψ φ θφ θ ψ φ ψ φ θ ψ φ ψ φ θ
θ ψ θ ψ θ
− − −⎡ ⎤⎢ ⎥+ − + =⎢ ⎥⎢ ⎥−⎣ ⎦
11 12 1321 22 2331 32 33
U U UU U UU U U
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
U as defined on the previous slide! (a function of arm-joint POSE and Desired End-Orientation)U
nive
rsiti
Kua
la L
umpu
r Mal
aysi
a Fr
ance
Inst
itute
Solving for Individual Orientation Angles (1st we will solve for ‘θ’ the middle one):
Selecting (3,3) Cθ = U33
With Cθ we “know” Sθ = ±(1 - C2θ).5
Hence: θ = Atan2(U33, ±(1-U332).5)
NOTE: leads to 2 solutions for θ!
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Re-examining the Matrices:
To solve for φ: Select terms: (1,3) & (2,3)CφSθ = U13
SφSθ = U23
Dividing the 2nd by the 1st: Sφ /Cφ = U23/U13
Tan(φ) = U23/U13
φ = Atan2(U13, U23)
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Continuing our Solution:
To solve for ψ: Select terms: (3,1) & (3,2)-SθCψ = U31
SθSψ = U32 (and dividing this by previous)Tan(ψ) = U32/-U31 (note sign migrates with term)ψ = Atan2(-U31, U32)
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
θ = Atan2(U33, ±(1-U332).5)
φ = Atan2(U13, U23)
ψ = Atan2(-U31, U32)
Summarizing:
Uij’s as defined on the earlier slide!
– and –
U is manipulator and desired orientation dependent
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Euler Angle RotationsRz1*Ry2*Rz3or
, , ,, ,z y zR R Rφ θ ϕ
Let consider a Spherical Wrist:
Z3
X3
Y3
Z4
X4
Y4
Z5
X5
Y5
Z6
X6
Y6
Here drawn in ‘Good Kinematic Home’ – for attachment to an Articulating Arm
Same Point
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
IKSing the Spherical Wrist
00d6θ6R65 6
+9000θ5R54 5
-9000θ4R43 4
αadΘVarLinkFrames
4
4 0 4 5 0 54 0 4 ; 5 5 0 5 ;
0 1 0 0 1 0
6 6 06 6 6 0
0 0 1
C S C SR S C R S C
C SR S C
−⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= = −⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦
−⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Writing The Solution:
11 12 13
21 22 23
31 32 33
4 0 4 5 0 5 6 6 04 0 4 5 0 5 6 6 0
0 1 0 0 1 0 0 0 1
C S C S C SS C S C S C
U U UU U UU U U
− −⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥• − • =⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
Uij’s as defined on the earlier slide!
– and –
U is manipulator and desired orientation dependent
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Let’s Solve (and try the ‘Pure Inverse’ Technique):
11 12 13
21 22 23
31 32 33
5 0 5 6 6 05 0 5 6 6 0
0 1 0 0 0 1
4 4 00 0 1
4 4 0
C S C SS C S C
C S U U UU U U
S C U U U
−⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− • =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦
⎡ ⎤⎡ ⎤⎢ ⎥⎢ ⎥− ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥−⎣ ⎦ ⎣ ⎦
Note: R4’s Inverse
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Simplifying
11 21 12 22 13 23
31 32 33
21 11 22 12 23 13
5 6 5 6 55 6 5 6 5
6 6 0
4 4 4 4 4 4
4 4 4 4 4 4
C C C S SS C S S C
S C
C U S U C U S U C U S UU U U
C U S U C U S U C U S U
−⎡ ⎤⎢ ⎥− − =⎢ ⎥⎢ ⎥⎣ ⎦
+ + +⎡ ⎤⎢ ⎥− − −⎢ ⎥⎢ ⎥− − −⎣ ⎦
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Solving:
lets select Term(3,3) on both sides:0 = C4U23 – S4U13
S4U13 = C4U23
Tan(θ4) = S4/C4 = U23/U13
θ4 = Atan2(U13, U23)
With the givens and back-substituted values (from the arm joints) we have a value for θ4 and the RHS is completely known!
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Solving for θ5 & θ6
For θ5: Select (1,3) & (2,3) termsS5 = C4U13 + S4U23
C5 = U33
Tan(θ5) = S5/C5 = (C4U13 + S4U23)/U33
θ5 = Atan2(U33, C4U13 + S4U23)
For θ6: Select (3,1) & (3, 2)S6 = C4U21 – S4U11
C6 = C4U22 – S4U12
Tan(θ6) = S6/C6 = ([C4U21 – S4U11]/[C4U22 – S4U12])θ6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11])
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Using the Pure Inversing:
We removed ambiguity from the solutionWe were able to solve for joints “In Order”Without noting – we see the obvious relationship between Spherical wrist and Euler Orientation!
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Summarizing:
θ4 = Atan2(U13, U23)
θ5 = Atan2(U33, C4U13 + S4U23)
θ6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11])
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Lets Try One:
Cylindrical Robot w/ Spherical WristGiven a Target matrix from Robot Mapping! (it’s an IKS after all!)The d3 “constant” is 400mm; the d6 offset (the ‘Hand Span’) is 150 mm.θ1 = Atan2((dx – ax*150),(dy-ay*150))d2 = (dz – az*150)d3 = ((dx – ax*150)2+p(dy-ay*150)2).5 - 400
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
The Frame Skeleton:
X
ZF0
F1 Z
X
X
F2 Z
Z
X
F6
F2.5
Z
X
F4
XF3
Z
F5
ZX
X
Z
Note “Dummy” Frame to account for Orientation problem with Spherical Wrist – might not be needed if we had set wrist kinematic home for Cylindrical Machine!U
nive
rsiti
Kua
la L
umpu
r Mal
aysi
a Fr
ance
Inst
itute
Solving for U:
11 1 0 0 0 1 0 0 1 0 1 01 1 0 1 0 0 1 0 0 1 0 0
0 0 1 0 1 0 0 1 0 0 0 1
x x x
y y y
z z z
C S n o aU S C n o a
n o a
−⎡ − − ⎤⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥= • − • − • − •⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎣ ⎦
NOTE: We needed a “Dummy Frame” to account for the Orientation issue at the end of the Arm (as drawn) – this
becomes a “part” of the arm space – not the wrist space!U
nive
rsiti
Kua
la L
umpu
r Mal
aysi
a Fr
ance
Inst
itute ( ) 13
0R−
Given from F Frame
Simplifying:
11 12 13
21 22 23
31 32 33
1 1 1 1 1 1
1 1 1 1 1 1
x z x z x z
y y y
x z x z x z
U U U C n S n C o S o C a S aU U U n o aU U U S n C n S o C n S a C a
+ + +⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥=⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− − −⎣ ⎦ ⎣ ⎦
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
Subbing Uij’s Into Spherical Wrist Joint Models:
θ4 = Atan2(U13, U23)= Atan2((C1ax + S1az), ay)
θ5 = Atan2(U33, C4U13 + S4U23)= Atan2{ (S1ax-C1az) , [C4(C1ax+S1az) + S4*ay]}
θ6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12]) = Atan2{[C4*ny - S4(C1nx+S1nz)],
[C4*oy - S4(C1ox+S1oz)]}
Uni
vers
itiK
uala
Lum
pur M
alay
sia
Fran
ce In
stitu
te
