Robotic Kinematics – the Inverse Kinematic Solution ME 3230 Kinematics and Mechatronics Dr. R. Lindeke ME 3230 11/8/2018

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 Geometry: Joint Space Cartesian Space Joint Space Cartesian Space ME 3230 11/8/2018

It a more difficult problem because: So this IKS Problem is Nasty (in Mathematics this is considered a Hard Modeling Issue) It a more difficult problem because: The Equation set is “Over-Specified”: 12 equations in 6 unknowns 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 ME 3230 11/8/2018

But the IKS is VERY Useful – some Uses of IKS include: Building Workspace Maps Allow “Off-Line Programming” solutions The IKS allows the engineer to equate Workspace capabilities with Programming realities to assure that execution is feasible (as done in ROBCAD & IGRIP) The IKS Aids in Workplace Design and Operational Simulations ME 3230 11/8/2018

Doing a Pure IKS solution: the R Manipulator R Frame Skeleton (as DH Suggest!) Same Origin Point! ME 3230 11/8/2018

LP Table and Ai’s Frames Link Var  d a  S C S  C  0  1 1 R  + 90 90 C1 -S1 1  2 2 P d2 + cl2 ME 3230 11/8/2018

FKS is A1*A2: ME 3230 11/8/2018

Forming The IKS: 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 equations that populate the LHS MATRIX ME 3230 11/8/2018

Forming The IKS: Form a ratio to build Tan(): Examining these two matrices n, o, a and d are “givens” 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! ME 3230 11/8/2018

After  is found, back substitute and solve for d2: 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 ME 3230 11/8/2018

Alternative Method – “doing a pure inverse approach” Form A1-1 then pre-multiply both side by this ‘inverse’ Leads to: A2 = A1-1*T0ngiven ME 3230 11/8/2018

Simplifying & 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 ME 3230 11/8/2018

Performing IKS For Industrial Robots: (a more involved problem) First let’s 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!) which is the z direction of the 3rd wrist joint This follows the DH Algorithm design tools as we have learned them! ME 3230 11/8/2018

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 ME 3230 11/8/2018

Performing IKS: Focus on Positioning We will define a point (called the WRIST CENTER) where all 3 z’s intersect as: Pc = [Px, Py, Pz] We find that this position is exactly: Pc = dtarget - dn*a Px = dtarget,x - dn*ax Py = dtarget,y - dn*ay Pz = dtarget,z - dn*az Note: dn is the so called ‘Handspan’ (a CONSTANT) ME 3230 11/8/2018

Treating the Arm Types (as separate robot entities): Cartesian (2 types) Cantilevered Gantry Cylindrical Spherical (w/o d2 offset) 2 Link Articulating (w/o d2 offset) ME 3230 11/8/2018

Cantilevered Cartesian Robot J2 J3 X0 P-P-P Configuration Y0 Z0 J1 ME 3230 11/8/2018

Gantry Cartesian Robot P-P-P Configuration X0 Z0 Y0 J3 J2 J1 ME 3230 11/8/2018

Cylindrical Robot Doing IKS – given a value for: X, Y and Z of End Compute , Z and R Z0 R Y0 X0 ME 3230 11/8/2018

Spherical Robot  Developing a IKS (model): Given Xe, Ye, & Ze Compute ,  & R R Z0 Y0 X0 ME 3230 11/8/2018

2-Link Articulating Arm Manipulator Z0 3 L2 L1 X0 Y0 2 1 ME 3230 11/8/2018

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} ME 3230 11/8/2018

Focusing on the ARM Manipulators in terms of Pc: Spherical: 1 = Atan2(Px, Py) 2 = Atan2( (Px2 + Py2).5 , Pz) D3 = (Px2 + Py2 + Pz2).5 – cl3 ME 3230 11/8/2018

Focusing on the ARM Manipulators in terms of Pc: Articulating: 1 = Atan2(Px, Py) 3 = Atan2(D, (1 – D2).5) Where D = 2 =  -   is: Atan2((Px2 + Py2).5, Pz)  is: ME 3230 11/8/2018

Focusing on the ARM Manipulators in terms of Pc: 2 = Where D = Atan2((Px2 + Py2).5, Pz) - ME 3230 11/8/2018

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 (like the 5 dof articulating robots in our lab!) This leads to two solutions for 1 the So-Called “Shoulder Left” and “Shoulder Right” solutions ME 3230 11/8/2018

Defining the d2 Offset issue 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!) ME 3230 11/8/2018

Lets look at this Device “From the Top” – a ‘plan’ view of the structure projected to the X0 Y0 plane F1  11 ME 3230 11/8/2018

Solving For 1: We will have a Choice of two poses for 1: ME 3230 11/8/2018

These lead to two 2’s (Spherical) This device (like the S110 robots in lab) 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 ME 3230 11/8/2018

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 ME 3230 11/8/2018

Note: target orientation is a ‘given’ for the IKS model! 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! ME 3230 11/8/2018

Focusing on Orientation Issues Lets begin by considering ‘Euler Angles’ (they are a model that is almost identical 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 ME 3230 11/8/2018

Euler Wrist Simplified: 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: NOTE: R03 is Manipulator dependent! (Inverse required a Transpose) ME 3230 11/8/2018

Continuing: to define the U-Matrix Rij are terms from the product of the first 3 Ai’s (rotational sub-matrix) 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 ME 3230 11/8/2018

Now Set: LHS (euler angles) = RHS (U-Matrix) U as defined on the previous slide! (a function of arm-joint POSE and Desired End-Orientation) ME 3230 11/8/2018

With C we “know” S = (1 - C2).5 Hence:  = Atan2(U33, (1-U332).5) Solving for Individual Orientation Angles (1st 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 ! ME 3230 11/8/2018

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) ME 3230 11/8/2018

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) ME 3230 11/8/2018

Summarizing:  = Atan2(U33, (1-U332).5)  = Atan2(U13, U23) Uij’s as defined on the earlier slide! – and – U is manipulator and desired orientation dependent ME 3230 11/8/2018

Let consider a Spherical Wrist: Same Origin Point Here drawn in ‘Good Kinematic Home’ – for attachment to an Articulating Arm ME 3230 11/8/2018

IKSing the Spherical Wrist Frames Link Var  d a  3  4 4 R 4 -90 4  5 5 5 +90 5  6 6 6 d6 ME 3230 11/8/2018

Writing The Solution: Uij’s as defined on the earlier slide! – and – U is manipulator and desired orientation dependent ME 3230 11/8/2018

Let’s Solve (here I try the ‘Pure Inverse’ Technique): Note: R4’s Inverse ME 3230 11/8/2018

Simplifying ME 3230 11/8/2018

Solving: Let’s 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! ME 3230 11/8/2018

Solving for 5 & 6 For 5: Select (1,3) & (2,3) terms C5 = U33 S5 = C4U13 + S4U23 C5 = U33 Tan(5) = S5/C5 = (C4U13 + S4U23)/U33 5 = Atan2(U33, C4U13 + S4U23) For 6: Select (3,1) & (3, 2) terms S6 = C4U21 – S4U11 C6 = C4U22 – S4U12 Tan(6) = S6/C6 = ([C4U21 – S4U11]/[C4U22 – S4U12]) 6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11]) ME 3230 11/8/2018

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! ME 3230 11/8/2018

Summarizing: 4 = Atan2(U13, U23) 5 = Atan2(U33, C4U13 + S4U23) 6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11]) ME 3230 11/8/2018

Lets Try One: Cylindrical Robot w/ Spherical Wrist Given would be 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 ME 3230 11/8/2018

The Frame Skeleton: 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! ME 3230 11/8/2018

Solving for U: 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! ME 3230 11/8/2018

Simplifying: ME 3230 11/8/2018

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)]} ME 3230 11/8/2018