L5 - Inverse Kinematics V1

FAB30703
Universiti Kuala Lumpur Malaysia France Institute
Inverse Kinematics Solution
Originally prepared by: Prof Engr Dr Ishkandar Baharin

Head 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:
Cartesian
Space
Joint
Space
In IKS we need Joint models from given End

Point Geometry:
Joint
Space Cartesian
Space
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
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
A Simple Example for a θ − r robot
θ −r
Given World Space Position (x, y), find the Joint Space Solution
Finding θ1
Revolute and
Prismatic Joints ⎛ y⎞
Combined θ1 = arctan ⎜ ⎟
⎝x⎠
More Specifically:
(x , y)
⎛ y⎞
θ1 = arctan 2 ⎜ ⎟ arctan2( ) specifies that
Y
⎝x⎠ it’s in the first quadrant
r Finding r:
θ1 X
r= ( x 2
+ y 2
)
Inverse Kinematics of a Two Link Manipulator
(x , y) Given: l1, l2 , x , y in World Space

θ2 Find: θ1 θ2 in Joint Space
l2 Redundancy:
A unique solution to this problem
does not exist. Notice, that using the
“givens” two solutions are possible.
θ1 l1
Sometimes no solution is possible.
l2 (x , y)
l2
l1 l1
The Geometric Solution
(x , y) Using the Law of Cosines:
θ2 l2
c 2 = a 2 + b 2 − 2ab cos C
2 2
( x 2 + y 2 ) = l1 + l2 − 2l1l2 cos(180 − θ 2 )
cos(180 − θ 2 ) = − cos(θ 2 )
2 2
l1 θ1 x 2 + y 2 − l1 − l2
α cos(θ 2 ) =
2l1l2
θ1
⎛ x 2 + y 2 − l12 − l2 2 ⎞
θ 2 = arccos⎜⎜ ⎟
⎟
Using the Law of Cosines: ⎝ 2l1l2 ⎠
sin B sin C Redundant since θ2 could be in the
=
b c first or fourth quadrant.
sinθ1 sin(180 − θ 2 ) sin(θ 2 )
= =
l2 x 2 + y2 x 2 + y2 Redundancy caused since θ2 has two
θ1 = θ1 − α possible values
θ1 = θ1 + α ⎛ l sin(θ ) ⎞ ⎛ y⎞
θ1 = arcsin⎜ 2 2 ⎟
+ arctan 2⎜ ⎟
⎛y⎞ ⎜ x 2 + y2 ⎟ ⎝x⎠
α = arctan 2 ⎜ ⎟ ⎝ ⎠
⎝x⎠
The Algebraic Solution
(x , y) Υ
θ2 l2
c 1 = c o sθ 1
c 1 + 2 = c o s(θ 1 + θ 2 )
l1 (1 ) x = l1 c 1 + l 2 c 1 + 2
θ1 (2 ) y = l1 s 1 + l 2 sin 1 + 2
(3 ) θ = θ 1 + θ 2
(1)2 + (2) 2 = x 2 + y 2 =
( 2 2 2
) ( 2 2 2
= l1 c1 + l2 (c1+ 2 ) 2 + 2l1l2 c1 (c1+ 2 ) + l1 s1 + l2 (sin1+ 2 ) 2 + 2l1l2 s1 (sin1+ 2 ) )
= l1 + l2 + 2l1l2 (c1 (c1+ 2 ) + s1 (sin1+ 2 ) )
2 2
2 2
= l1 + l2 + 2l1l2 c 2 Only Unknown
Note:
⎛ x 2 + y 2 − l12 − l2 2 ⎞ cos(a−+b) = (cosa)(cosb)−+ (sina)(sinb)
∴ θ 2 = arccos⎜⎜ ⎟
⎟
⎝ 2 l l
1 2 ⎠ sin(a−+b) = (cosa)(sinb)+− (cosb)(sina)
x = l1 c 1 + l 2 c 1 + 2 Note:
= l1 c 1 + l 2 c 1 c 2 − l 2 s 1 s 2 cos(a−+b) = (cosa)(cosb)−+ (sina)(sinb)
sin(a−+b) = (cosa)(sinb)+− (cosb)(sina)
= c 1 ( l1 + l 2 c 2 ) − s 1 ( l 2 s 2 )
y = l1 s 1 + l 2 sin 1+ 2 We know what θ2 is from the previous

= l1 s 1 + l 2 s 1 c 2 + l 2 s 2 c 1 slide. We need to solve for θ1 . Now
= c 1 ( l 2 s 2 ) + s 1 ( l1 + l 2 c 2 ) we have two equations and two
unknowns (sin θ1 and cos θ1 )
x + s1 ( l 2 s 2 )
c1 =
( l1 + l 2 c 2 )
Substituting for c1 and simplifying
x + s1 ( l 2 s 2 )
y= ( l 2 s 2 ) + s 1 ( l1 + l 2 c 2 ) many times
( l1 + l 2 c 2 )
=
1
( l1 + l 2 c 2 )
(
x l 2 s 2 + s 1 ( l1 2 + l 2 2 + 2 l1 l 2 c 2 ) ) Notice this is the law of
cosines and can be
replaced by x2+ y2
y ( l1 + l 2 c 2 ) − x l 2 s 2 ⎛ y ( l1 + l 2 c 2 ) − x l 2 s 2 ⎞
s1 = θ 1 = arcsin ⎜⎜ ⎟
x2+ y2 x2+ y2 ⎟
⎝ ⎠
Doing a Pure IKS solution: the θR
Manipulator
X1 X2
Same Point!
Y0 Z1 Z2
Y1 Y2
X0
Z0
θR Frame Skeleton (as DH

Suggest!)
LP Table and Ai’s
Frames Link Var θ d a α Sα Cα Sθ Cθ
0ª1 1 R θ+ 0 0 90 1 0 C1 -S1
90
1ª2 2 P 0 d2 + 0 0 1 0 1 0
cl2
⎡ − S1 0 C1 0⎤ ⎡1 0 0 0 ⎤
⎢ C1 0 S1 0⎥ ⎢0 1 0 0 ⎥
A1 = ⎢ ⎥ A2 = ⎢ ⎥
⎢ 0 1 0 0⎥ ⎢0 0 1 d 2 + cl2 ⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1⎦ ⎣0 0 0 1 ⎦
FKS is A1*A2:
⎡ − S1 0 C1 0 ⎤ ⎡1 0 0 0 ⎤
⎢ C1 0 S1 0 ⎥ ⎢ 0 1 0 0 ⎥
⎢ ⎥•⎢ ⎥=
⎢ 0 1 0 0⎥ ⎢0 0 1 d 2 + cl2 ⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1 ⎦ ⎣0 0 0 1 ⎦
⎡ − S1 0 C1 C1(d 2 + cl2 ) ⎤
⎢ C1 0 S1 S1(d 2 + cl2 ) ⎥
⎢ ⎥
⎢ 0 1 0 0 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
Forming The IKS:
o
F ⎡On ,o ,a Dx , y , z ⎤
d
F =⎢ ⎥
z0 a ⎣ 0 1 ⎦
n KNOWN
y0
x0
⎡ − S1 0 C1 C1( d 2 + cl2 ) ⎤ ⎡ nx ox ax dx ⎤
⎢ C1 0 S1 S1( d 2 + cl2 ) ⎥ ⎢ n y oy ay dy ⎥
⎢ ⎥ =⎢ ⎥
⎢ 0 1 0 0 ⎥ ⎢ nz oz az dz ⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1 ⎦ ⎣0 0 0 1⎦
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
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 θ: If 2 Matrices are
(1,4): C1*(d2+cl2) = dx Equal then EACH
and EVERY term is
(2,4): S1*(d2+cl2) = dy also Uniquely
Form a ratio to build Tan(θ): equal as well!
S1/C1 = dy/ dx
Tan θ = dy/dx
θ = Atan2(dx, dy)
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
Alternative Method – “doing a pure
inverse approach”
Form A1-1 then pre-multiply both side by

this ‘inverse’
Leads to: A2 = A1-1*T0ngiven
⎡1 0 0 0 ⎤ ⎡ − S1 C1 0 0 ⎤ ⎡ nx ox ax dx ⎤
⎢0 1 0 0 ⎥ ⎢ 0 0 1 0⎥ ⎢ ny oy ay dy ⎥
⎢ ⎥=⎢ ⎥•⎢ ⎥
⎢0 0 1 d 2 + cl2 ⎥ ⎢ C1 S1 0 0 ⎥ ⎢ nz oz az dz ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣0 0 0 1 ⎦ ⎣ 0 0 0 1⎦ ⎣ 0 0 0 1⎦
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
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 point
The n Frame is offset from this Z’s intersection
point at a distance dn (the hand span) along the a
vector of the desired solution (3rd column of
desired orientation sub-matrix)
This follows the DH Algorithm design tools as we
have learned them!
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 Joints
Joints 4 to 6 in a full functioning spherical wrist
Wrist Joints function as a primary tool to ORIENT
the end frame as required by the desired target
POSE
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*a
Px = dtarget,x - dn*ax
Note: dn is the so called
Py = dtarget,y - dn*ay ‘Handspan’ (a CONSTANT)
Pz = dtarget,z - dn*az
Focusing on the ARM Manipulators
in terms of Pc:
Prismatic:
q1 = d1= Pz (its along Z0!) – cl1
q2 = d2 = Px or Py - cl2
q3 = d3= Py or Px - cl3
Cylindrical:
θ1 = Atan2(Px, Py)
d2 = Pz – cl2
d3 = Px/C1 – cl3 {or +(Px2 + Py2).5 – cl3}
in terms of Pc:
Spherical:
θ1 = Atan2(Px, Py)
θ2 = Atan2( (Px2 + Py2).5 , Pz)
D3 = (Px2 + Py2 + Pz2).5 – cl3
in terms of Pc:
Articulating:
θ1 = Atan2(Px, Py)
θ3 = Atan2(D, ±(1 – D2).5)
Where D = (P x
2
+ Py2 + Pz2 − a22 − a32 )
2 a2 a3
θ2 = φ-α
φ is: Atan2((Px2 + Py2).5, Pz)
A tan 2(sin α )=
cos α
α is:
( )

⎜ 2 3 (
⎛ 2a a ± 1 − D 2
) ⎞
⎟
A tan 2 ⎜
⎜ ( Px + Py + Pz + a2 − a3 ) ⎟
2 2 2 2 2 ⎟
⎝ ⎠
in terms of Pc:
θ2 = Atan2((Px2 + Py2).5, Pz) -
(
⎜
A tan 2 ⎜
2 3 (
⎛ 2a a ± 1 − D 2
) ) ⎞
⎟
⎜ ( Px + Py + Pz + a2 − a3 ) ⎟
2 2 2 2 2 ⎟
⎝ ⎠
Where D = ( P x
2
+ Py2 + Pz2 − a22 − a32 )
2 a 2 a3
One Further Complication Must Be
Considered:
This is called the d2 offset problem

A d2 offset is a problem that states that the
nth frame has a non-zero offset along the Y0
axis as observed in the solution of the T0n
with all joints at home
This leads to two solutions for θ1 the
So-Called “Shoulder Left” and “Shoulder
Right” solutions
Defining the d2 Offset issue
The Zpc
The ARM Wrist
Z0 d 2 Xpc
Y0, Z1
Ypc
X0, X1
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!)
Lets look at this Device “From the Top” – a plan
view of the structure projected to the X0 Y0 plane
Pc'(Px, Py)
Y0
a3'
d2
Z1 a 2'
R' φ1
α1 X1
X 1 = R′
d2
θ1
X0
Z1'
X1'
Solving For θ1:
We will have a Choice of two poses for θ1:

θ 11 = φ1 − α1
θ 11 = A tan 2( X pc , Y pc )
(
− A tan 2 ( X pc
2
+ Y pc2 − d 22 ) , d 2
.5
)
θ 12 = 180° + α1 + φ1 =
180° + A tan 2 ( )
( X pc2 + Y pc2 − d 22 ) , d 2 +
.5
A tan 2 ( X pc , Y pc )
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 & Down
Shoulder Left Î Elbow Up & Down
Now, lets look at Orientation IKS’s
Orientation IKS again relies on separation

of joint effects
We (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
These Ideas Lead to an Orientation
Model for a Device that is Given by:
R R = Rgiven
3
0
6
3
This ‘model’ separates Arm Joint and Wrist

Joint Contribution to the desired Target
Orientation (note target orientation is a
given for the IKS model!)
Focusing on Orientation Issues
Lets begin by considering

⎡ cos φ − sin φ 0⎤
‘Euler Angles’ (they are a
Rz1 = ⎢ sin φ cos φ 0⎥
model that is almost ⎢ ⎥
⎢⎣ 0 0 1 ⎥⎦
identical to a full functioning
Spherical Wrist as defined ⎡ cosθ 0 sin θ ⎤
using the D-H algorithm!): Ry 2 = ⎢ 0 1 0 ⎥
⎢ ⎥
Step 1, Form a Product: ⎢⎣ − sin θ 0 cosθ ⎥⎦
Rz1*Ry2*Rz3 ⎡ cosψ − sinψ 0 ⎤
This product becomes R36 in Rz 3 = ⎢ sinψ cosψ 0 ⎥
⎢ ⎥
the model on the previous ⎢⎣ 0 0 1 ⎥⎦
slide
Euler Wrist Simplified:
⎡Cφ Cθ Cψ − Sφ Sψ −Cφ Cθ Sψ − Sφ Cψ Cφ Sθ ⎤
⎢ Sφ 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:
⎡ nx ox ax ⎤
( R0 ) ⎢ny
3 −1 ⎢ ⎥
NOTE: R03 is
oy ay
⎥
Manipulator
dependent!
Inverse requires
its Transpose
⎢⎣ nz oz az ⎥⎦ given
Simplifying the U Matrix: (a RHS
matrix relative to the R36 Euler Product)
⎡ R11 R12 R13 ⎤

⎢
R0 = R 21 R 22 R 23
3 ⎥
⎢ ⎥
⎢⎣ R31 R32 R33⎥⎦
∴
⎡ R11 R 21 R31⎤
( R0 ) = ⎢ R12 R22 R32⎥
3 −1 ⎢ ⎥
⎢⎣ R13 R 23 R33⎥⎦
(this is a transpose!)
Continuing: to define the U-Matrix
⎡U11 U12 U13 ⎤

⎢U 21 U 22 U 23⎥ =
⎢ ⎥
⎢⎣U 31 U 32 U 33⎥⎦
⎡ nx R11 + ny R21 + nz R31 ox R11 + oy R 21 + oz R31 ax R11 + a y R21 + az R31 ⎤
⎢ ⎥
n
⎢ x R 12 + n y R 22 + nz R32 ox R12 + o y R 22 + oz R 32 a x R12 + a y R 22 + a z R32 ⎥
⎢ nx R13 + ny R23 + nz R33 ox R13 + oy R23 + oz R33 ax R13 + a y R23 + az R33 ⎥
⎣ ⎦
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
Now Set
LHS (Euler angles) = RHS (U-Matrix):
⎡Cφ Cθ Cψ − Sφ Sψ −Cφ Cθ Sψ − Sφ Cψ Cφ Sθ ⎤
⎢ Sφ Cθ Cψ + Cφ Sψ − Sφ Cθ Sψ + Cφ Cψ ⎥
Sφ Sθ =
⎢ ⎥
⎢⎣ − Sθ Cψ Sθ Sψ Cθ ⎥⎦
⎡U 11 U 12 U 13 ⎤
⎢U 21 U 22 U 23⎥
⎢ ⎥ U as defined on the
previous slide! (a
⎢⎣U 31 U 32 U 33 ⎥⎦ function of arm-joint
POSE and Desired End-
Orientation)
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 θ!

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)
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)
Summarizing:
Euler Angle
θ = Atan2(U33, ±(1-U33 2).5)
Rotations
φ = Atan2(U13, U23) Rz1*Ry2*Rz3
or
Rz ,φ , Ry ,θ , Rz ,ϕ
ψ = Atan2(-U31, U32)
Uij’s as defined on the earlier slide!

– and –
U is manipulator and desired orientation
dependent
Let consider a Spherical Wrist:
Same Point
Z4
Here drawn in
Y4 Y5
‘Good Kinematic
Home’ – for
attachment to X4
an Articulating Y3 X5
Arm
Z5
X3 Y6
Z3
X6
Z6
IKSing the Spherical Wrist
Frames Link Var Θ d a α
3ª4 4 R θ4 0 0 -90
4ª5 5 R θ5 0 0 +90
5ª6 6 R θ6 d6 0 0
⎡C 4 0 − S 4 ⎤ ⎡C 5 0 S 5 ⎤
R 4 = ⎢ S 4 0 C 4 ⎥ ; R 5 = ⎢ S 5 0 − C 5 ⎥ ;
⎢ ⎥ ⎢ ⎥
⎢⎣ 0 − 1 0 ⎥⎦ ⎢⎣ 0 1 0 ⎥⎦
⎡C 6 − S 6 0 ⎤
R 6 = ⎢ S 6 C 6 0 ⎥
⎢ ⎥
⎢⎣ 0 0 1 ⎥⎦
Writing The Solution:
⎡C 4 0 −S 4⎤ ⎡C5 0 S 5 ⎤ ⎡C 6 −S 6 0⎤
⎢ S 4 0 C 4 ⎥ • ⎢ S 5 0 −C5⎥ • ⎢ S 6 C 6 0⎥ =
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣ 0 −1 0 ⎥⎦ ⎢⎣ 0 1 0 ⎥⎦ ⎢⎣ 0 0 1⎥⎦
⎡U11 U12 U13 ⎤
⎢U U U ⎥
⎢ 21 22 23 ⎥ U ’s as defined on the earlier slide!
ij
⎢⎣U 31 U 32 U 33 ⎥⎦ – and –
U is manipulator and desired orientation
dependent
Let’s Solve (and try the ‘Pure Inverse’ Technique):
⎡C 5 0 S 5 ⎤ ⎡C 6 − S 6 0 ⎤
⎢ S5 ⎥ ⎢
0 −C 5 • S 6 C 6 0 = ⎥
⎢ ⎥ ⎢ ⎥
⎢⎣ 0 1 0 ⎥⎦ ⎢⎣ 0 0 1 ⎥⎦
⎡ C4 S 4 0 ⎤ ⎡U11 U12 U13 ⎤
Note:
R4’s
Inverse
⎢ 0 ⎥ ⎢
0 −1 U 21 U 22 U 23 ⎥
⎢ ⎥⎢ ⎥
⎢⎣ − S 4 C 4 0 ⎥⎦ ⎢⎣U 31 U 32 U 33 ⎥⎦
Simplifying
⎡C5C 6 −C5S 6 S 5 ⎤
⎢ S 5C 6 −S 5S 6 −C5⎥ =
⎢ ⎥
⎢⎣ S 6 C6 0 ⎥⎦
⎡C 4U11 + S 4U 21 C 4U12 + S 4U 22 C 4U13 + S 4U 23 ⎤
⎢ −U31 −U 32 −U 33 ⎥
⎢ ⎥
⎢⎣C 4U 21 − S 4U11 C 4U 22 − S 4U12 C 4U 23 − S 4U13 ⎥⎦
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!
Solving for θ5 & θ6
For θ5: Select (1,3) & (2,3) terms

S5 = 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])
Using the Pure Inversing:
We removed ambiguity from the

solution
We were able to solve for joints “In
Order”
Without noting – we see the obvious
relationship between Spherical wrist
and Euler Orientation!
Summarizing:
θ4 = Atan2(U13, U23)
θ5 = Atan2(U33, C4U13 + S4U23)
θ6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11])

Lets Try One:
Cylindrical Robot w/ Spherical Wrist

Given 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
The Frame Skeleton:
F4
Z
X
F2.5 X F3 X
F5
F2 Z
X
X Z
Z Z
F6
F1 Z
Z
F0 Z
X
Note “Dummy” Frame to account for Orientation
X
problem with Spherical Wrist – might not be needed if
we had set wrist kinematic home for Cylindrical
Machine!
Solving for U:
Given from F Frame
(R ) 3 −1
−1
⎡⎡C1 −S1 0⎤ ⎡ 0 0 −1⎤ ⎡ 0 0 1⎤ ⎡ 0 1 0⎤ ⎤ ⎡ nx ox ax ⎤
⎢ ⎥
U = ⎢⎢ S1 C1 0⎥ • ⎢−1 0 0 ⎥ • ⎢−1 0 0⎥ • ⎢−1 0 0⎥ ⎥ • ⎢ny oy ay ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣⎢⎣ 0 0 1⎥⎦ ⎢⎣ 0 −1 0 ⎥⎦ ⎢⎣ 0 −1 0⎥⎦ ⎢⎣ 0 0 1⎥⎦ ⎥⎦ ⎢⎣ nz oz az ⎥⎦
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!
Simplifying:
⎡U11 U12 U13 ⎤ ⎡C1nx + S1nz C1ox + S1oz C1ax + S1az ⎤

⎢U U U ⎥ = ⎢ ny oy ay ⎥
⎢ 21 22 23 ⎥ ⎢ ⎥
⎢⎣U31 U32 U33 ⎥⎦ ⎢⎣ S1nx − C1nz S1ox − C1nz S1ax − C1az ⎥⎦
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)]}

L5 - Inverse Kinematics V1

Transféré par

Informations du document

Description originale:

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

L5 - Inverse Kinematics V1

Transféré par

Droits d'auteur :

Formats disponibles

FAB30703

Universiti Kuala Lumpur Malaysia France Institute

Inverse Kinematics Solution

Originally prepared by: Prof Engr Dr Ishkandar Baharin

 In FKS we built a tool for finding end frame

 In IKS we need Joint models from given End

 It is a more difficult problem because:

 Building Workspace Map

 Allow “Off-Line Programming” solutions

 The IKS allows the engineer to equate

 The IKS Aids in Workplace Design and

(x , y) Given: l1, l2 , x , y in World Space

θ2 Find: θ1 θ2 in Joint Space

y = l1 s 1 + l 2 sin 1+ 2 We know what θ2 is from the previous

θR Frame Skeleton (as DH

Frames Link Var θ d a α Sα Cα Sθ Cθ

 Examining these two matrices

 Form a ratio to build Tan(θ): equal as well!

 After θ is found, back substitute

 Form A1-1 then pre-multiply both side by

 Selecting and Equating (1,4)

 First lets consider the concept of the

 We can now separate the POSE effects:

 We will define a point (called the

 θ2 = Atan2((Px2 + Py2).5, Pz) -

 This is called the d2 offset problem

 We will have a Choice of two poses for θ1:

 We have two θ1’s

 Or four θ2’s and θ3’s in the Articulating

 Orientation IKS again relies on separation

 This ‘model’ separates Arm Joint and Wrist

 Lets begin by considering

⎡ R11 R12 R13 ⎤

⎡U11 U12 U13 ⎤

Rij are terms from the product of the first 3 Ai’s

 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 θ!

 To solve for φ: Select terms: (1,3) & (2,3)

 To solve for ψ: Select terms: (3,1) & (3,2)

Uij’s as defined on the earlier slide!

Frames Link Var Θ d a α

 lets select Term(3,3) on both sides:

 For θ5: Select (1,3) & (2,3) terms

 For θ6: Select (3,1) & (3, 2)

 We removed ambiguity from the

 θ5 = Atan2(U33, C4U13 + S4U23)

 θ6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11])

 Cylindrical Robot w/ Spherical Wrist

NOTE: We needed a “Dummy Frame” to account for the

⎡U11 U12 U13 ⎤ ⎡C1nx + S1nz C1ox + S1oz C1ax + S1az ⎤

 θ5 = Atan2(U33, C4U13 + S4U23)

 θ6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12])

Vous aimerez peut-être aussi

In FKS we built a tool for finding end frame

In IKS we need Joint models from given End

It is a more difficult problem because:

Building Workspace Map

Allow “Off-Line Programming” solutions

The IKS allows the engineer to equate

The IKS Aids in Workplace Design and

Examining these two matrices

Form a ratio to build Tan(θ): equal as well!

After θ is found, back substitute

Form A1-1 then pre-multiply both side by

Selecting and Equating (1,4)

First lets consider the concept of the

We can now separate the POSE effects:

We will define a point (called the

θ2 = Atan2((Px2 + Py2).5, Pz) -

This is called the d2 offset problem

We will have a Choice of two poses for θ1:

We have two θ1’s

Or four θ2’s and θ3’s in the Articulating

Orientation IKS again relies on separation

This ‘model’ separates Arm Joint and Wrist

Lets begin by considering

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 θ!

To solve for φ: Select terms: (1,3) & (2,3)

To solve for ψ: Select terms: (3,1) & (3,2)

lets select Term(3,3) on both sides:

For θ5: Select (1,3) & (2,3) terms

For θ6: Select (3,1) & (3, 2)

We removed ambiguity from the

θ5 = Atan2(U33, C4U13 + S4U23)

θ6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11])

Cylindrical Robot w/ Spherical Wrist

θ5 = Atan2(U33, C4U13 + S4U23)

θ6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12])