Manipulator Dynamics (2)

Read Chapter 6 Dynamic equations for each link

• For each link there are two equations to describe the effects of force and torque to the motion: • Newton equation

= 𝑖𝑖 𝑐𝑐𝑖𝑖 • Euler’s equation𝐹𝐹 𝑚𝑚𝑣𝑣̇

τ2 = + × 𝑖𝑖 𝐶𝐶𝑖𝑖 𝑖𝑖 𝑖𝑖 𝐶𝐶𝑖𝑖 𝑖𝑖 τ xc1 𝑁𝑁 𝐼𝐼 𝜔𝜔̇ 𝜔𝜔 𝐼𝐼 𝜔𝜔 1 {Ci} has its origin at the center of mass of the x link and has the same orientation as the link 1 frame {i} Dynamic equations for all the links

• Use the iterative Newton-Euler algorithm i F zi zi+1

xi+1

xi i +1n i +1 i n f i f i N • Between links there are action and reaction forces (torques) • For link n, we have n+1 f = 0 and n +1n = 0; therefore the equations can start from the last link and go inwards Compute forces and torques

• Look the top figure in the previous slide, one has:

= 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 = 𝑖𝑖 × × 𝐹𝐹 𝑓𝑓 − 𝑅𝑅𝑖𝑖+1 𝑓𝑓 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖+1 𝑁𝑁 𝑛𝑛 − 𝑅𝑅𝑖𝑖+1 𝑛𝑛 − 𝑃𝑃𝐶𝐶𝑖𝑖 𝐹𝐹 − 𝑃𝑃 𝑅𝑅𝑖𝑖+1 𝑓𝑓 • From the above, one can obtain the following iterative equations:

= + 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖+1 𝑓𝑓 = 𝐹𝐹 + 𝑅𝑅 𝑓𝑓+ × + × 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖+1 𝑛𝑛 𝑁𝑁 𝑅𝑅𝑖𝑖+1 𝑛𝑛 𝑃𝑃𝐶𝐶𝑖𝑖 𝐹𝐹 𝑃𝑃 𝑅𝑅𝑖𝑖+1 𝑓𝑓 • Note that n+1 f = 0 and n +1n = 0; so we can start the iteration from link n.

• Since every joint has only one torque applied, we have

= ( ) 𝑖𝑖 𝑇𝑇 𝑖𝑖 Question: how the above𝜏𝜏𝑖𝑖 equation𝑛𝑛 should𝑍𝑍̂ be changed for a prismatic joint? The iterative Newton-Euler dynamics algorithm (1) • The iterative algorithm takes two steps - First step: perform outward iterations to compute velocities and accelerations - Second step: perform inward iterations to computer forces and torques • First step: - Angular acceleration

= + 𝐴𝐴 𝐴𝐴 𝐴𝐴 𝐵𝐵 = + ( ) 𝛺𝛺𝑐𝑐 𝛺𝛺𝐵𝐵 𝑅𝑅𝐵𝐵 𝛺𝛺𝑐𝑐 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖 𝑖𝑖+1 = + ( ) 𝑅𝑅𝑖𝑖 𝜃𝜃̇𝑖𝑖+1 𝑍𝑍̂ 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖 𝑖𝑖+1 𝜔𝜔 𝜔𝜔 𝜔𝜔 𝑅𝑅𝑖𝑖 𝜔𝜔 𝜃𝜃̇𝑖𝑖+1 𝑍𝑍̂= + × + - Linear acceleration𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖 𝑖𝑖+1 𝑖𝑖 𝑖𝑖+1 𝑖𝑖+1 𝜔𝜔̇ 𝑅𝑅𝑖𝑖 𝜔𝜔̇ 𝑅𝑅𝑖𝑖 𝜃𝜃̇𝑖𝑖+1 𝑍𝑍̂ 𝜃𝜃̈𝑖𝑖+1 𝑍𝑍̂ = ( + × ) 𝜔𝜔 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑣𝑣 𝑅𝑅𝑖𝑖 𝑣𝑣 𝜔𝜔 = 𝑃𝑃𝑖𝑖+1 [ × + ×( × ) + ] - For the𝑖𝑖+1 center𝑖𝑖+ of1 the 𝑖𝑖link: 𝑖𝑖 𝑖𝑖+1 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖 𝑣𝑣̇ 𝑅𝑅𝑖𝑖 𝜔𝜔̇ 𝑅𝑅𝑖𝑖+1 𝑃𝑃 𝜔𝜔 𝜔𝜔 𝑅𝑅𝑖𝑖+1 𝑃𝑃 𝑣𝑣̇ = × + ×( × )+ 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1 𝑣𝑣̇𝐶𝐶𝑖𝑖+1 𝜔𝜔̇ 𝑃𝑃𝐶𝐶𝑖𝑖+1 𝜔𝜔 𝜔𝜔 𝑃𝑃𝐶𝐶𝑖𝑖+1 𝑣𝑣̇ The iterative Newton-Euler dynamics algorithm (2) • Second step:

- Draw the Newton-Euler equations = 𝑖𝑖+1 𝑖𝑖+1 𝐹𝐹 𝑚𝑚𝑖𝑖+1 𝑣𝑣̇𝐶𝐶𝑖𝑖+1 = + × 𝑖𝑖+1 𝐶𝐶𝑖𝑖+1 𝑖𝑖+1 𝑖𝑖+1 𝐶𝐶𝑖𝑖+1 𝑖𝑖+1 - Inward𝑁𝑁 iteration𝐼𝐼 from𝜔𝜔 ̇link n to𝜔𝜔 link 1:𝐼𝐼 𝜔𝜔

= + 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖+1 𝑓𝑓 = 𝐹𝐹 + 𝑅𝑅 𝑓𝑓 + × + × 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖 𝑖𝑖+1 𝑖𝑖+1 𝐶𝐶𝑖𝑖 𝐹𝐹 𝑃𝑃𝑖𝑖+1 𝑖𝑖+1 𝑓𝑓 𝑛𝑛= ( 𝑁𝑁) 𝑅𝑅 𝑛𝑛 𝑃𝑃 𝑅𝑅 𝑖𝑖 𝑇𝑇 𝑖𝑖 𝜏𝜏𝑖𝑖 𝑛𝑛 𝑍𝑍̂ Iterative algorithm example

• Use the OSU hexapod as an example which is similar to the manipulator shown below – to simplify the problem,

let m1 and m2 be point mass and located at the tip of the link

τ1 m2 Ze

Xe D1 Z1 m1 X1

X2 D2 Z2

D 0 Z0 X0 Use the two iterations • Determine the values needed in the algorithm - The position vectors of the center of mass

= 1 𝐶𝐶1 𝑃𝑃 = 𝐷𝐷1𝑋𝑋�1 2 𝐶𝐶2 - The inertia tensor𝑃𝑃 at𝐷𝐷2 the𝑋𝑋�2 center of mass = 0 𝐶𝐶1 𝐼𝐼1 = 0 𝐶𝐶2 - Rotational matrices𝐼𝐼2

0 0 = 0 = 0 1 1 2 2 1 𝑐𝑐0 𝑠𝑠0 1 2 𝑐𝑐0 1 𝑠𝑠0 𝑅𝑅0 −𝑠𝑠1 𝑐𝑐1 𝑅𝑅1 −𝑠𝑠2 𝑐𝑐2 − • Outward iteration - For link 1

0 0 = = 0 = = 0 1 1 1 1 ω 𝜃𝜃̇ 𝑍𝑍̂ 𝜔𝜔̇ 𝜃𝜃1̈ 𝑍𝑍̂1 1 𝜃𝜃̇ 𝜃𝜃1̈ 0 0 0 0 ( ) ( ) = 0 0 = 0 = + 0 2 + = 2 1 1 1 1 1 ̇ 1 1 𝑐𝑐0 𝑠𝑠0 1 1 0 −𝐷𝐷 0𝜃𝜃̇ 1 −𝐷𝐷 𝜃𝜃 𝐶𝐶1 𝑣𝑣̇ −𝑠𝑠1 𝑐𝑐1 𝑣𝑣̇ 𝐷𝐷1𝜃𝜃̈1 𝑣𝑣̇ 𝐷𝐷1𝜃𝜃̈1 𝑔𝑔 𝑔𝑔 𝑔𝑔

( ) ( ) 0 = 2 = 2 1 ̇ 1 1 1 ̇ 1 = 0 1 −𝐷𝐷 𝜃𝜃 −𝑚𝑚 𝐷𝐷 𝜃𝜃 1 1 0 𝐹𝐹 𝑚𝑚 𝐷𝐷1𝜃𝜃̈1 𝑚𝑚1𝐷𝐷1𝜃𝜃̈1 𝑁𝑁 𝑔𝑔 𝑚𝑚1𝑔𝑔 - For link 2

0 + = + = + 0 = 2 ̇1 𝑠𝑠2𝜃𝜃̇1 = + 2 2 𝑠𝑠 0𝜃𝜃 2 1 2 2 1 1 1 1 2 2 2 1 𝑐𝑐 𝜃𝜃̇ 𝜃𝜃̇ 𝑠𝑠 𝜃𝜃̈ ω 𝑅𝑅 𝜃𝜃̇ 𝑍𝑍̂ 𝜃𝜃̇ 𝑍𝑍̂ 𝑐𝑐2𝜃𝜃̇1 𝑐𝑐 𝜃𝜃̇ 2 ̇2 𝜔𝜔̇ −𝑠𝑠2𝜃𝜃̇1𝜃𝜃̇2 𝑐𝑐2𝜃𝜃̈1 𝜃𝜃 𝜃𝜃̇2 0 ( ) ( ) 𝜃𝜃̈2 = 0 2 = ( ) 2 2 2 1 ̇ 1 −𝑐𝑐2𝐷𝐷1 𝜃𝜃̇ 1 2 𝑐𝑐0 1 𝑠𝑠0 −𝐷𝐷 𝜃𝜃 2 𝑣𝑣̇ −𝑠𝑠2 𝑐𝑐2 𝐷𝐷1𝜃𝜃̈1 𝑠𝑠2𝐷𝐷1 𝜃𝜃̇ 1 − 𝑔𝑔 1 ̈1 −𝐷𝐷 𝜃𝜃= × + × × + 2 2 2 2 2 2 2 𝑣𝑣̇𝐶𝐶2 𝜔𝜔̇ 𝑃𝑃𝐶𝐶2 𝜔𝜔 𝜔𝜔 𝑃𝑃𝐶𝐶2 𝑣𝑣̇ + ( ) = + × 0 + × × 0 + ( ) 2 + 𝑐𝑐2𝜃𝜃̇1𝜃𝜃̇2 𝑠𝑠2𝜃𝜃̈1 2 𝑠𝑠2𝜃𝜃̇1 𝑠𝑠2𝜃𝜃̇1 2 −𝑐𝑐2𝐷𝐷1 𝜃𝜃̇ 1 2 2 𝐷𝐷0 𝐷𝐷0 2 𝑠𝑠0𝑔𝑔 2 𝑣𝑣̇𝐶𝐶 −𝑠𝑠2𝜃𝜃̇1𝜃𝜃̇2 𝑐𝑐2𝜃𝜃̈1 𝑐𝑐2𝜃𝜃̇1 𝑐𝑐2𝜃𝜃̇1 𝑠𝑠2𝐷𝐷1 𝜃𝜃̇ 1 𝑐𝑐2𝑔𝑔 𝜃𝜃̈2 𝜃𝜃̇2 𝜃𝜃̇2 −𝐷𝐷1𝜃𝜃̈1

0 + + 2 2 2 2 2 = + 2 + ( ) 2+ = 2 −𝑐𝑐2 𝐷𝐷2 𝜃𝜃̇1 − 𝐷𝐷2 𝜃𝜃̇2 2 1 ̇1 2 −𝑐𝑐2 𝐷𝐷2+𝜃𝜃̇1 − 𝐷𝐷2 𝜃𝜃̇2 +− 𝑐𝑐2𝐷𝐷1 𝜃𝜃̇1 + 𝑠𝑠2𝑔𝑔 2 2 −𝑐𝑐 𝐷𝐷 𝜃𝜃 𝑠𝑠 𝑔𝑔 2 2 𝐶𝐶2 2 ̈2 2 𝑣𝑣̇ 𝐷𝐷 𝜃𝜃 𝑐𝑐2𝑠𝑠2𝐷𝐷2 𝜃𝜃̇1 𝑠𝑠2𝐷𝐷1 𝜃𝜃̇1 𝑐𝑐2𝑔𝑔 𝐷𝐷2𝜃𝜃̈2 𝑐𝑐2𝑠𝑠2𝐷𝐷2 𝜃𝜃̇1 𝑠𝑠2𝐷𝐷1 𝜃𝜃̇1 𝑐𝑐2𝑔𝑔 2 2 1 2 2 2 1 𝐷𝐷 𝑠𝑠 𝜃𝜃̇ 𝜃𝜃̇ −𝑐𝑐 𝐷𝐷 𝜃𝜃̈ 1 1 𝐷𝐷2𝑠𝑠2𝜃𝜃̇1𝜃𝜃̇2 −𝐷𝐷 𝜃𝜃̈ 2𝐷𝐷2𝑠𝑠2𝜃𝜃̇1𝜃𝜃̇2 −𝑐𝑐2 𝐷𝐷2𝜃𝜃̈1 −𝐷𝐷1 𝜃𝜃̈1 + 0 = 2 2 2 2 = 0 −𝑐𝑐2 𝐷𝐷2+𝜃𝜃̇1 − 𝐷𝐷2 𝜃𝜃̇2 +− 𝑐𝑐2𝐷𝐷1 𝜃𝜃̇1 + 𝑠𝑠2𝑔𝑔 2 2 2 2 2 0 𝐹𝐹 𝑚𝑚 𝐷𝐷2𝜃𝜃̈2 𝑐𝑐2𝑠𝑠2𝐷𝐷2 𝜃𝜃̇1 𝑠𝑠2𝐷𝐷1 𝜃𝜃̇1 𝑐𝑐2𝑔𝑔 𝑁𝑁 2𝐷𝐷2𝑠𝑠2𝜃𝜃̇1𝜃𝜃̇2 −𝑐𝑐2 𝐷𝐷2𝜃𝜃̈1 −𝐷𝐷1 𝜃𝜃̈1 • Inward iteration - For link 2

= 2 2 𝑓𝑓 𝐹𝐹 0 = 0 × = ( ) 2 2 𝐷𝐷0 2 ( + + + ) 2 2 2 2 ̇1 ̇2 2 2 ̈1 1 ̈1 𝑛𝑛 𝑓𝑓 𝑚𝑚 −𝐷𝐷 2𝐷𝐷 𝑠𝑠 𝜃𝜃 𝜃𝜃 −2𝑐𝑐 𝐷𝐷 𝜃𝜃 −𝐷𝐷 2𝜃𝜃 𝐷𝐷2 𝐷𝐷2𝜃𝜃̈2 𝑐𝑐2𝑠𝑠2𝐷𝐷2 𝜃𝜃̇1 𝑠𝑠2𝐷𝐷1 𝜃𝜃̇1 𝑐𝑐2𝑔𝑔 - For link 1

0 + ( ) 2 2 2 = + = 0 0 1 2 + + + + 2 2 2 −𝑐𝑐2 𝐷𝐷2 𝜃𝜃̇1 − 𝐷𝐷2 𝜃𝜃̇2 − 𝑐𝑐2𝐷𝐷1 𝜃𝜃̇1 𝑠𝑠2𝑔𝑔 1 1 ̇ 1 1 1 2 1 𝑐𝑐 −𝑠𝑠 0 2 2 −𝑚𝑚 𝐷𝐷 𝜃𝜃 2 2 𝑓𝑓 𝑅𝑅 𝑓𝑓 𝐹𝐹 − 𝑚𝑚 𝐷𝐷2𝜃𝜃̈2 𝑐𝑐2𝑠𝑠2𝐷𝐷2 𝜃𝜃̇1 𝑠𝑠2𝐷𝐷1 𝜃𝜃̇1 𝑐𝑐2𝑔𝑔 𝑚𝑚1𝐷𝐷1𝜃𝜃̈1 𝑠𝑠2 𝑐𝑐2 1 2𝐷𝐷2𝑠𝑠2𝜃𝜃̇1𝜃𝜃̇2 −𝑐𝑐2 𝐷𝐷2𝜃𝜃̈1 −𝐷𝐷1 𝜃𝜃̈1 𝑚𝑚 𝑔𝑔 = + × + × Figure out the details? 1 1 2 1 1 1 1 2 𝑛𝑛 𝑅𝑅2 𝑛𝑛 𝑃𝑃𝐶𝐶1 𝐹𝐹 𝑃𝑃2 𝑅𝑅2 𝑓𝑓 Final dynamic equation

• Since every joint has only one degree of freedom, one has:

= ( ) 1 𝑇𝑇 1 𝜏𝜏1 = ( 𝑛𝑛) 𝑍𝑍̂ 2 𝑇𝑇 2 Centrifugal force • Ultimately one𝜏𝜏2 can obtain:𝑛𝑛 𝑍𝑍̂ Coriolis force

= M( ) + V( , )+G( )

• where 𝜏𝜏 Θ Θ̈ Θ Θ̇ Θ Gravity force = = 𝜃𝜃1 𝜏𝜏1 Θ 𝜏𝜏 2 𝜃𝜃2 𝜏𝜏 Lagrangian formulation

• Another approach is using the Lagrangian formulation - Step 1 obtain the Lagrangian of a manipulator ( , )= ( , ) - ( )

where𝐿𝐿 Θ Θ(̇ , 𝑘𝑘) =Θ Θ̇ =𝑢𝑢 Θ ( ) + ( ) 𝑛𝑛 𝑛𝑛 1 𝑇𝑇 1 𝑖𝑖 𝑇𝑇 𝐶𝐶𝑖𝑖 𝑖𝑖 is the kinetic energy of the manipulator,𝑖𝑖 and𝑖𝑖 𝑘𝑘 Θ Θ̇ ∑𝑖𝑖 𝑘𝑘𝑖𝑖 ∑𝑖𝑖 2 𝑚𝑚𝑖𝑖 𝑣𝑣𝐶𝐶 𝑣𝑣𝐶𝐶 2 𝜔𝜔 𝐼𝐼 𝜔𝜔 ( )= = [ ( ) ] 𝑛𝑛 𝑛𝑛 0 𝑇𝑇 0 is the potential energy of the manipulator𝑖𝑖 𝑢𝑢 Θ ∑𝑖𝑖 𝑢𝑢𝑖𝑖 ∑𝑖𝑖 −𝑚𝑚𝑖𝑖 𝑔𝑔 𝑃𝑃𝐶𝐶 • The dynamic equation can be expressed as

- = - + = 𝑑𝑑 𝜕𝜕𝐿𝐿 𝜕𝜕𝜕𝜕 𝑑𝑑 𝜕𝜕𝑘𝑘 𝜕𝜕𝑘𝑘 𝜕𝜕𝑢𝑢 Becomes the same as the one ̇ 𝑑𝑑𝑡𝑡 𝜕𝜕Θ 𝜕𝜕Θ 𝜏𝜏 𝑑𝑑𝑡𝑡 𝜕𝜕Θ̇ 𝜕𝜕Θ 𝜕𝜕Θ shown𝜏𝜏 earlier = M( ) + V( , )+G( )

𝜏𝜏 Θ Θ̈ Θ Θ̇ Θ