Robot Modeling and the Forward Kinematic Solution

1. Robot Modeling and the Forward Kinematic Solution ME 3230 R. R. Lindeke

2. Looking Closely at the T0n Matrix • At the end of our discussion of “Robot Mapping” we found that the T0n matrix related the end of the arm frame (n) to its base (0) – • Thus it must contain information related to the several joints of the robot • It is a 4x4 matrix populated by complex equations for both position and orientation (POSE)

3. Looking Closely at the T0n Matrix • To get at the equation set, we will choose to add a series of coordinate frames to each of the joint positions • The frame attached to the 1st joint is labeled 0 – the base frame! – while joint two has Frame 1, etc. • The last Frame is the end or n Frame

4. Looking Closely at the T0n Matrix • As we have seen earlier, we can define a HTM (T(i-1)i) to the transformation between any two SO3 based frames • Thus we will find that the T0n is given by a product formed by:

5. Looking Closely at the T0n Matrix • For simplicity, we will redefine each of the of these transforms (T(i-1)i) as Ai • Then, for a typical 3 DOF robot Arm, T0n = A1*A2*A3 • While for a full functioned 6 DOF robot (arm and wrist) would be: T0n = A1*A2*A3*A4*A5*A6A1 to A3 ‘explain’ the arm joint effect while A4 to A6 explain the wrist joint effects

6. Looking At The Frame To Frame Arrangements – Building A Modeling Basis • When we move from one frame to another, we will encounter: • Two translations (in a controlled sense) • Two rotations (also in a controlled sense) • A rotation and translation WRT the Zi-1 • These are called the Joint Parameters • A rotation and translation WRT the Xi • These are called the Link Parameters

7. A model of the Joint Parameters NOTE!!!

8. A model of the Link Parameters ai or

9. Talking Specifics – Joint Parameters • i is an angle measured about the Zi-1 axis from Xi-1 to Xi and is a variable for a revolute joint – its fixed for a Prismatic Joint • di is a distance measured from the origin of Frame(i-1) to the intersection of Zi-1 and Xi and is a variable for a prismatic joint – its fixed for a Revolute Joint

10. Talking Specifics – Link Parameters • ai (or li) is the Link length and measures the distance from the intersection of Zi-1 to the origin of Framei measured along Xi • i is the Twist angle which measures the angle from Zi-1 to Zi as measured about Xi • Both of these parameters are fixed in value regardless of the joint type. • A Further note: Fixed does not mean zero degrees or zero length just that they don’t change

11. Very Important to note: • Two Design Axioms prevail in this modeling approach • Axiom DH1: The Axis Ximust be designed to be perpendicular to Zi-1 • Axiom DH2: The Axis Ximust be designed to intersect Zi-1 • Thus, within reason we can design the structure of the coordinate frames to simplify the math (they are under our control!)

12. Returning to the 4 ‘Frame-Pair’ Operators: • 1st is  which is an operation of pure rotation about Z or: • 2nd is d which is a translation along Z or:

13. Returning to the 4 Frame Operators: • 3rd is a Translation Along X or: • 4th is  which is a pure Rotation about X or:

14. The Overall Effect is:

15. Simplifying this Matrix Product: This matrix is the general transformation relating each and every of the frame pairs along a robot structure

16. So, Since We Can Control the Building of this Set Of Frames, What Are The Rules? • We will employ a method called the Denavit-Hartenberg Method • It is a Step-by-Step approach for modeling each of the frames from the initial (or 0) frame all the way to the n (or end) frame • This modeling technique makes each joint axis (either rotation or extension) the Z-axis of the appropriate frame (Z0 to Zn-1). • The Joint motion, thus, is taken W.R.T. the Zi-1 axis of the frame pair making up the specific transformation matrix under design

17. The D-H Modeling Rules: • Locate & Label the Joint Axes: Z0 to Zn-1 • Establish the Base Frame. Set Base Origin anywhere on the Z0 axis. Choose X0 and Y0 conveniently and to form a right hand frame. • Locate the origin Oi where the common normal to Zi-1 and Zi intersects Zi. If Zi intersects Zi-1 locate Oi at this intersection. If Zi-1 and Zi are parallel, locate Oi at Joint i+1

18. The D-H Modeling Rules: • Establish Xi along the common normal between Zi-1 and Zithrough Oi, or in the direction Normal to the plane Zi-1 – Zi if these axes intersect • Establish Yi to form a right hand system Set i = i+1, if i<n loop back to step 3(Repeat Steps 3 to 5 for I = 1 to I = n-1) • Establish the End-Effector (n) frame: OnXnYnZn. Assuming the n-th joint is revolute, set kn = a along the direction Zn-1. Establish the origin On conveniently along Zn, typically mounting point of gripper or tool. Set jn = o in the direction of gripper closure (opening) and set in = n such that n = oxa. Note if tool is not a simple gripper, set Xn and Yn conveniently to form a right hand frame.

19. The D-H Modeling Rules: • Create a table of “Link” parameters: • i as angle about Zi-1 between X’s • di as distance along Zi-1 • i as angle about Xi between Z’s • ai as distance along Xi • Form HTM matrices A1, A2, … An by substituting , d,  and a into the general model • Form T0n = A1*A2*…*An

20. Some Issues to remember: • If you have parallel Z axes, the X axis of the second frame runs perpendicularly between them • When working on a revolute joint, the model will be simpler if the two X directions are in alignment at “Kinematic Home” – ie. Our model’s starting point • To achieve this kinematic home, rotate the model about moveable axes (Zi-1’s) to align X’s • Kinematic Home is not particularly critical for prismatic joints • An ideal model will have n+1 frames • However, additional frames may be necessary – these are considered ‘Dummy’ frames since they won’t contain joint axes

21. Applying D-H to a General Case:

22. General Case: Considering Link i • Connects Frames: i-1 and I and includes Joint i • This information allowed us to ‘Build’ The L.P. (link parameter) Table as seen here

23. Leads to an Ai Matrix:

24. Frame Skeleton for Prismatic Hand Robot

25. LP Table: Depends on Location of n(end)-frame!

26. Leading to 5 Ai Matrices

27. #5 is: This value is called the Hand Span and depends on the Frame Skeleton we developed • Now, Lets Form the FKS: T0n = A1*A2*A3*A4*A5 • I’ll use a software: Mathematica

28. Solving for FKS • Here we have a special case – two of the Joints are a “planer arm” revolute model – i.e. parallel, consecutive revolute joints • These are contained in the A2 and A3 Matrices • These should be pre-multiplied using a trigonometric tool that recognizes the sum of angle cases ((Full)Simplify in mathematica) • Basically then: T0n = A1*{A2A3}*A4*A5

29. Finalizing the FKS – perform a physical verification • Physical verification means to plug known angles into the variables and compute the Ai’s and FKS against the Frame Skeleton

30. Another? 6dof Articulating Arm – (The Figure Contains Frame Skelton)

31. LP Table * With End Frame in Better Kinematic Home. Here, as shown, it would be (6 - 90), which is a problem!

32. A Matrices

33. A Matrices, cont. At Better Kinematic Home!

34. Leads To: • FKS of: After Preprocessing A2-A3-A4, with (Full)Simplify function, the FKS must be reordered as A1-A234-A5-A6 and Simplified

35. Solving for FKS • Pre-process {A2*A3*A4} with Full Simplify • They are the “planer arm” issue as in the previous robot model • Then Form: A1* {A2*A3*A4}*A5*A6 • Simplify for FKS!

36. Simplifies to(in the expected Tabular Form): nx = C1·(C5·C6·C234 - S6·S234) - S1·S5·C6 ny = C1·S5·C6 + S1·(C5·C6·C234 - S6·S234) nz = S6·C234 + C5·C6·S234 ox = S1·S5·S6 - C1·(C5·S6·C234 + C6·S234) oy = - C1·S5·S6 - S1·(C5·S6·C234 + C6·S234) oz = C6·C234 - C5·S6·S234 ax = C1·S5·C234 + S1·C5 ay = S1·S5·C234 - C1·C5 az = S5·S234 dx = C1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) + d6·S1·C5 dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5 dz = S234·(d6·S5 + a4) + a3·S23 + a2·S2

37. Verify the Model • Again, substitute known’s (typically 0 or 0 units) for the variable kinematic variables • Check each individual A matrix against your robot frame skeleton sketch for physical agreement • Check the simplified FKS against the Frame skeleton for appropriateness

38. After Substitution: • nx = C1·(C5·C6·C234 - S6·S234) - S1·S5·C6 = 1(1-0) – 0 = 1 • ny = C1·S5·C6 + S1·(C5·C6·C234 - S6·S234) = 0+ 0(1 – 0) = 0 • nz = S6·C234 + C5·C6·S234 = 0 + 0 = 0 • ox = S1·S5·S6 - C1·(C5·S6·C234 + C6·S234) = 0 – 1(0 + 0) = 0 • oy = - C1·S5·S6 - S1·(C5·S6·C234 + C6·S234) = -0 – 0(0 + 0) = 0 • oz = C6·C234 - C5·S6·S234 = 1 – 0 = 1 • ax = C1·S5·C234 + S1·C5 = 0 + 0 = 0 • ay = S1·S5·C234 - C1·C5 = 0 – 1 = -1 • az = S5·S234 = 0 • dx = C1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) + d6·S1·C5= 1*(1(0 + a4) + a3 + a2) + 0 = a4 + a3 + a2 • dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5= 0(1(0 + a4) + a3 + a2) – d6 = -d6 • dz = S234·(d6·S5 + a4) + a3·S23 + a2·S2= 0(0 + a4) + 0 + 0 = 0