Inverse Kinematics –IKS Solutions ME 4135 – Robotics and Controls R.R. Lindeke, Ph.D., Fall 2011

FKS vs. 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 POSE Geometry: Joint Space Cartesian Space Joint Space Cartesian Space

So this IKS is ‘Nasty’ – it is a so-call Hard Mathematical Problem It a more difficult problem because: – The Equation set is “Over-Specified” 12 equations in 6 unknowns – Space may be “Under-Specified” for example ‘Planer devices’ with more joints than 2 – The Solution set can contain Redundancies Thus they exhibit Multiple solutions – The Solution Sets may be un-defined Unreachable in 1 or many joints

But the IKS in Critical in Robotics Builds Workspace limits and controller maps Allows “Off-Line Programming” solutions Thus, compares Workspace capabilities with Programming desirability to assure that execution is feasible Aids in Workplace design and operational simulations

Doing a Pure IKS solution: the  R Manipulator  R Frame Skeleton

LP Table and A i ’s FramesLinkVar  dl  SS CC S  C  0 → 11R  C1-S1 1 → 22P0d2 + cl

FKS is A1*A2:

Forming The IKS:

Examining the two sides of this FKS equation – n, s, a and d are given in an inverse sense – since we desire to put the manipulator on a particular target – But we will build the general solution only Term (1, 4) & (2,4) both side allow us to find an equation for  : Select (1,4): C1*(d 2 +cl 2 ) = d x Select (2,4): S1*(d 2 +cl 2 ) = d y Form a ratio of Term 2,4 by Term 1,4 to build an equation for Tan(  ) and then  : S1/C1 = d y / d x Tan  = d y /d x  = Atan2(d x, d y )

Forming The IKS: After  is found, back substitute and solve for d 2 : Choose term(1,4): C1*(d 2 +cl 2 ) = d x Isolating d 2 : d 2 = [d x /C1] - cl 2

Alternative Method – doing a pure inverse approach Form [A 1 ] -1 then pre-multiply both side by this ‘inverse’ Leads to: A2 = A 1 -1 *T 0 n given

After Simplifying the RHS:

Solving: Selecting and Equating terms (1,4) 0 = -S1*d x + C1*d y Solving: S1*d x = C1*d y Tan(  ) = (S1/C1) = (d y /d x )  = Atan2(d x, d y ) Selecting and Equating terms (3,4) -- after back substituting  solution – and note, after the above step,  is known as an angle d 2 + cl 2 = C1*d x + S1*d y d 2 = C1*d x + S1*d y - cl 2 d 2 = Cos[Atan2(d x, d y )]*d x + Sin[Atan2(d x, d y )]*d y –cl 2

Performing IKS For Industrial Robots: First lets consider the previously defined Spherical Wrist simplification – All Wrist joint Z’s intersect at a point – The n Frame is offset from this intersection as a distance d n along the a vector of the desired solution (3 rd column of desired orientation sub- matrix) – This is as expected by the DH Algorithm

Performing IKS We can now separate the effects of the ARM joints – Joints 1 to 3 in a full function manipulator (without redundant joints) – They function to position the spherical wrist at a target POSITION related to the desired target POSE Arm Joints are separated from the 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 (the WRIST CENTER) as: – P c = [P x, P y, P z ] – Here we define P c = d target - d n *a P x = d target,x - d n *a x P y = d target,y - d n *a y P z = d target,z - d n *a z

Focusing on the ARM Manipulators in terms of P c : Prismatic: q 1 = P z (its along Z 0 !) – cl1 q 2 = P x or P y - cl2 q 3 = P y or P x - cl3 Cylindrical:  1 = Atan2(P x, P y ) d 2 = P z – cl2 d 3 = P x *C1 – cl3 or +(P x 2 + P y 2 ).5 – cl3

Focusing on the ARM Manipulators in terms of P c : Spherical:  1 = Atan2(Px, Py)  2 = Atan2( (P x 2 + P y 2 ).5, P z ) D3 = (P x 2 + P y 2 + P z 2 ).5 – cl3

Focusing on the ARM Manipulators in terms of P c : Articulating:  1 = Atan2(Px, Py)  3 = Atan2(D,  (1 – D 2 ).5 ) – Where D =  2 =  -  –  is: Atan2((Px 2 + Py 2 ).5, P z ) –  is:

One Further Complication: This is called the d 2 offset problem A d 2 offset is a problem that states that the n frame has a non-zero offset along the Y 0 axis as observed with all joints at home, in the solution of the 0 T n This leads to two solutions for  1 the So-Called Shoulder Left and Shoulder Right solutions

Defining the d 2 Offset issue Here: ‘The ARM’ might be a prismatic joint as in the Stanford Arm or it might be l 2 & l 3 links in an Articulating Arm and 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” L2’L2’ L3’L3’   11  X1X1

Solving For  1 We will have a Choice (of two) poses for  :

In this so-called “Hard Arm” We have two  1 ’s These lead to two  2 ’s (Spherical) Or to four  2 ’s and  3 ’s in the Articulating Arm Shoulder Right Elbow Up & Down Shoulder Left Elbow Up & Down

The Orientation Model Evolves from: Separates Arm Joint and Wrist Joint Contribution to Target (given) orientation

Focusing on Orientation Issues Lets begin by considering Euler Angles (they are a model that is almost identical to a full functioning Spherical Wrist): Form Product: – R z1 *R y2 *R z3 – This becomes R 3 6

Euler Wrist Simplified: And this matrix is equal to a U matrix prepared by multiplying the inverse of the ARM joint orientation matrices inverse and the Desired (given) target orientation NOTE: R 0 3 is Manipulator dependent!

Simplifying the RHS: (our so-called U Matrix)

Continuing:

Finally:

Solving for Individual Orientation Angles (1 st  ): Selecting (3,3) → C  = U 33 With C  we “know” S  =  (1-C 2  ).5 Hence:  = Atan2(U 33,  (1-U 33 2 ).5 NOTE: 2 solution for  !

Re-examining the Matrices: To solve for  : Select terms: (1,3) & (2,3)  C  S  = U 13  S  S  = U 23  Dividing the 2 nd by the 1 st : S  /C  = U 23 /U 13  so Tan(  ) = U 23 /U 13  leading to:  = Atan2(U 13, U 23 )

Continuing our Solution: To solve for  : Select terms: (3,1) & (3,2)  -S  C  = U 31  S  S  = U 32  so Tan(  ) = U 32 /-U 31 (note the ‘minus’ sign track the appropriate term!)  And Thus  = Atan2(-U 31, U 32 )

Summarizing:  = Atan2(U 33,  (1-U 33 2 ).5  = Atan2(U 13, U 23 )  = Atan2(-U 31, U 32 )

Let’s examine a Spherical Wrist:

IKSing the Spherical Wrist FramesLinkVar  dl  3 → 44R 4 → 55R 5 → 66R 66 d6d6 00

Writing The Solution:

Lets See (By Pure Inverse) Technique:

Simplifying

Solving: After Examination here let’s select (3,3) both sides: 0 = C4U 23 – S4U 13 S4U 13 = C4U 23 Tan(  4 ) = S4/C4 = U 23 /U 13  4 = Atan2(U 13, U 23 ) With the given and back-substituted values (from the arm joints) we have a value for  4 the RHS is completely known

Solving for  5 &  6 For  5 : Select (1,3) & (2,3) terms S5 = C4U 13 + S4U 23 C5 = U 33 Tan(  5 ) = S5/C5 = (C4U 13 + S4U 23 )/U 33  5 = Atan2(U 33, C4U 13 + S4U 23 ) For  6 : Select (3,1) & (3, 2) S6 = C4U 21 – S4U 11 C6 = C4U 22 – S4U 12 Tan(  6 ) = S6/C6 = ([C4U 21 – S4U 11 ],[C4U 22 – S4U 12 ])  6 = Atan2 ([C4U 21 – S4U 11 ], [C4U 22 – S4U 12 ])

Summarizing:  4 = Atan2(U 13, U 23 )  5 = Atan2(U 33, C4U 13 + S4U 23 )  6 = Atan2 ([C4U 21 – S4U 11 ], [C4U 22 – S4U 12 ])

Lets Try One: Cylindrical Robot w/ Spherical Wrist Given a Target matrix (it’s an IKS after all!) The d 3 “constant” is 400mm; the d 6 offset (call it the ‘Hand Span’) is 150 mm.  1 = Atan2((d x – a x *150),(d y -a y *150)) d2 = (dz – a z *150) d3 = ((d x – a x *150) 2,(d y -a y *150) 2 )

The Frame Skeleton: Note “Dummy” Frame to account for Orientation problem with Spherical Wrist

Solving for U: NOTE: We needed a “Dummy Frame” to account for the Orientation issue at the end of the Arm

Simplifying:

Subbing U ij ’s Into Spherical Wrist Joint Models:  4 = Atan2(U 13, U 23 ) = Atan2((C1a x + S1a z ), (S1a x -C1a z ))  5 = Atan2(U 33, C4U 13 + S4U 23 ) = Atan2{a y, [C4(C1a x +S1a z ) + S4 (S1a x -C1a z )]}  6 = Atan2 ([C4U 21 - S4U 11 ], [C4U 22 - S4U 12 ]) = Atan2{[C4(S1n x -C1n z ) - S4(C1n x +S1n z )], [C4(S1s x -C1s z ) - S4(C1s x +S1s z )]}