Dynamics of Open Chains: Newton Euler Approach

Home , Dynamics (mechanics), Inverse dynamics

ECE5463: Introduction to Robotics Lecture Note 13: Dynamics of Open Chains: Newton Euler Approach

Prof. Wei Zhang

Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA

Spring 2018

Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 1 / 22 Outline

Introduction •

Newton-Euler Inverse Dynamics of Open Chains •

Forward Dynamics of Open Chains •

Outline Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 2 / 22 Lagrangian vs. Newton-Euler Methods

• We have introduced the Lagrangian approach in the previous lecture. This lecture will introduce the Newton-Euler method

• Recall the diﬀerences between the two methods:

Lagrangian Formulation Newton-Euler Formulation - Energy-based method - Balance of forces/torques - Dynamic equations in closed - Dynamic equations in form numeric/recursive form - Often used for study of dynamic - Often used for numerical properties and analysis of solution of forward/inverse control methods dynamics

Introduction Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 3 / 22 Main Ideas for Inverse Dynamics

• Goal: Given state (θ, θ˙) and acelleration θ¨, compute the required τ Rn ∈ τ = M(θ)θ¨ + h(θ, θ˙)

• We will not drive the analytical expressions for the overall M(θ) and h(θ, θ˙).

• Instead, we aim to write dynamic equations separately for each link/body.

total ˙ T Chapter 8. Dynamicsi of= Openi Chainsi [ad i ] ( i i) 293 F G V − V G V joint axis i ˙ Vi Vi

i { }

Fi AdT ( ) − Ti+1,i Fi+1

Figure 8.6: Free-body diagram illustrating the moments and forces exerted on link i.

Introduction Substituting this resultLecture into 13 Equation (ECE5463 Sp18)(8.46), we get Wei Zhang(OSU) 4 / 22 ˙ ¨ ˙ ˙ i = iθi + [AdTi,i 1 ] i 1 + [ad i ] iθi, (8.47) V A − V − V A i.e., the acceleration of link i is the sum of three components: a component due to the joint acceleration θï, a component due to the acceleration of link i 1 expressed in i , and a velocity-product component. − Once we have{ } determined all the link twists and accelerations moving out- ward from the base, we can calculate the joint torques or forces by moving inward from the tip. The rigid-body dynamics (8.40) tells us the total wrench that acts on link i given and ˙ . Furthermore, the total wrench acting on Vi Vi link i is the sum of the wrench i transmitted through joint i and the wrench applied to the link through jointFi + 1 (or, for link n, the wrench applied to the link by the environment at the end-effector frame n + 1 ), expressed in the frame i. Therefore, we have the equality { } ˙ T T i i ad ( i i) = i AdT ( i+1); (8.48) G V − Vi G V F − i+1,i F see Figure 8.6. Solving from the tip toward the base, at each link i we solve for the only unknown in Equation (8.48): . Since joint i has only one degree Fi of freedom, five dimensions of the six-vector i are provided “for free” by the structure of the joint, and the actuator onlyF has to provide the scalar force or torque in the direction of the joint’s screw axis:

τ = T . (8.49) i Fi Ai Equation (8.49) provides the torques required at each joint, solving the inverse dynamics problem.

May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org Main Ideas for Inverse Dynamics (Continued)

• Note that i can be easily computed for each link. Thus, the main task is to ﬁnd , ˙ Gand the total wrench for each link in terms of the given (θ, θ,˙ θ¨). Vi Vi

• This can be done recursively:

- The velocity Vi of link i is determined by Vi−1 of link i − 1 and the joint rate θ˙i of joint i

- The acceleration V˙ i of link i is determined by V˙ i−1 of link i − 1 and the acceleration θ¨i of joint i

- The total wrench of link i can be determined by gravity and the wrench of adjacent links using the principle of action and reaction:

Wrench applied by body i to body i+1 = − Wrench applied by body i+1 to body i

Introduction Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 5 / 22 Outline

Introduction •

Newton-Euler Inverse Dynamics of Open Chains •

Forward Dynamics of Open Chains •

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 6 / 22 Some Notations (1/3)

• For each link i = 1, . . . , n, Frame i is attached to its center of mass. { }

• The base frame is denoted 0 { }

• The end-effector frame is denoted n + 1 . Its configuration in n is fixed. { } { }

• M : Conﬁguration of j in i when the robot is at home position i,j { } { }

• T (θ): Conﬁguration of j in i when the robot is at θ position i,j { } { }

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 7 / 22 Some Notations (2/3) • All the following quantities are expressed in frame i { }

- Vi: Twist of link {i}

- ¯mi: mass; Ii: rotational inertia matrix; Chapter 8. Dynamics of Open Chains 293

joint axis i ˙ Vi Ii 0 i - Gi = : Spatial inertia matrix V 0 ¯miI

i { }

Fi AdT ( ) - Ai: Screw axis of joint i, expressed in {i} − Ti+1,i Fi+1

Figure 8.6: Free-body diagram illustrating the moments and forces exerted on link i. - F : wrench transmitted through i = (mi, fi) Substituting this result into Equation (8.46), we get joint i to link {i} ˙ ¨ ˙ ˙ i = iθi + [AdTi,i 1 ] i 1 + [ad i ] iθi, (8.47) V A − V − V A i.e., the acceleration of link i is the sum of three components: a component due to the joint acceleration θï, a component due to the acceleration of link i 1 expressed in i , and a velocity-product component. − Once we have{ } determined all the link twists and accelerations moving out- ward from the base, we can calculate the joint torques or forces by moving inward from the tip. The rigid-body dynamics (8.40) tells us the total wrench that acts on link i given and ˙ . Furthermore, the total wrench acting on Vi Vi link i is the sum of the wrench i transmitted through joint i and the wrench applied to the link through jointFi + 1 (or, for link n, the wrench applied to the link by the environment at the end-effector frame n + 1 ), expressed in the Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18)frame i. Therefore, we have the equality Wei Zhang(OSU){ } 8 / 22 ˙ T T i i ad ( i i) = i AdT ( i+1); (8.48) G V − Vi G V F − i+1,i F see Figure 8.6. Solving from the tip toward the base, at each link i we solve for the only unknown in Equation (8.48): . Since joint i has only one degree Fi of freedom, five dimensions of the six-vector i are provided “for free” by the structure of the joint, and the actuator onlyF has to provide the scalar force or torque in the direction of the joint’s screw axis:

τ = T . (8.49) i Fi Ai Equation (8.49) provides the torques required at each joint, solving the inverse dynamics problem.

May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org Some Notations (3/3) • Some simple facts: −1 −1 - Mi−1,i = Mi−1Mi and Mi,i−1 = Mi Mi−1

h i - Ai = AdM 1 Si, where Si is the screw axis of joint i (relative to {0} frame) i− when θ = 0

[A ]θ −[A ]θ - Ti−1,i(θ) = Ti−1,i(θi) = Mi−1,ie i i and Ti,i−1(θi) = e i i Mi,i−1

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 9 / 22 Forward Propagation of Twist (1/3)

• Goal: Given i 1 and (θi, θ˙i), we want to ﬁnd i V − V

• Generally speaking, there are two key elements in deﬁning a rigid body velocity: - The inertia frame relative to which the motion is considered

- The representation frame in which the coordinate of the velocity vector is determined.

• So far, we have mainly played with the representation frames, while the motion is always considered to be relative to the ﬁxed s frame. { }

• Sometimes, it is convenient consider a relative velocity with respect to a moving frame.

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 10 / 22 Forward Propagation of Twist (2/3) b/c • Deﬁne a as the velocity (twist) of b relative to c , expressed in a . V { } { } { }

• Connecting to our previous shorthand notations:

= b/s, = b/s, = i/s Vs Vs Vb Vb Vi Vi

• With Newtonian approximation: we have

c/s = b/s + c/b V∗ V∗ V∗

- The ”*” can be replaced with {s} or {b} or any other stationary frames.

- Note the representation frame must be the same for all the three twists.

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 11 / 22 Forward Propagation of Twist (3/3) • Now consider velocity of link i: we will have

i/s i/i 1 i 1/s = − + − Vi Vi Vi

i/i 1 • Note that i − can be viewed as the body twist of a 1-joint robot where V i/i 1 i-1 is viewed as the “ﬁxed” frame − = θ˙ { } ⇒V i Ai i • Moreover, with our shorthand notation:

i−1/s i−1/s Vi = AdTi,i 1 Vi−1 = AdTi,i 1 Vi−1 − −

• Therefore, velocity (twist) can be propagated by: ˙ i = iθi + AdTi,i 1 i 1 (1) V A − V −

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 12 / 22 Forward Propagation of Acceleration (1/2)

• Goal: Given ˙ i 1, i 1 and (θi, θ˙i, θ¨i), we want to ﬁnd ˙ i, V − V − V

• We ﬁrst mention a result about the derivative of the adjoint operator.

• Given a time-varying T (t) = (R(t), p(t)), the associated adjoint AdT (t) is also a time-varying matrix. We have the following result about its derivative:

[ ]θ(t) Derivative of adjoint: If T (t) = e V M, where is a constant twist and M is a constant matrix in SE(3), and θ(t) is a scalarV time-varying function. Then

d AdT (t) = ad θ˙ AdT (t) (2) dt V

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 13 / 22 Forward Propagation of Acceleration (2/2) • With the result in (2), we can propagate acceleration by taking derivative on both sides of (1):

˙ ¨ ˙ d i = iθi + AdTi,i 1 i 1 + AdTi,i 1 i 1 V A − V − dt − V − ˙ ¨ ˙ ˙ i = iθi + AdTi,i 1 i 1 + [ad i ] iθi ⇒V A − V − V A

• Thus, the acceleration of line i has three components: - a component due to joint acceleration θ¨ - a component due to the acceleration of link i − 1 expressed in {i} - a velocity-product component

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 14 / 22 Backward Propagation of Wrenches

• Suppose we have computed the twist and acceleration ˙ for all the links Vi Vi

• Now we can compute forces/torques. The Newton-Euler Eq. for link i is:

T ˙ T Fi − AdTi+1,i Fi+1 = GiVi − [adVi ] (GiVi)

• If we know , we can solve for . Fi+1 Fi Chapter 8. Dynamics of Open Chains 293

joint axis i ˙ Vi • We can compute all the wrenches Vi backward from joint n to joint 1 Fi i { } T • Each joint has 1 dof τi = i i i F AdT ( ) ⇒ F A − Ti+1,i Fi+1

Figure 8.6: Free-body diagram illustrating the moments and forces exerted on link i.

Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 15 / 22 Substituting this result into Equation (8.46), we get ˙ ¨ ˙ ˙ i = iθi + [AdTi,i 1 ] i 1 + [ad i ] iθi, (8.47) V A − V − V A i.e., the acceleration of link i is the sum of three components: a component due to the joint acceleration θï, a component due to the acceleration of link i 1 expressed in i , and a velocity-product component. − Once we have{ } determined all the link twists and accelerations moving out- ward from the base, we can calculate the joint torques or forces by moving inward from the tip. The rigid-body dynamics (8.40) tells us the total wrench that acts on link i given and ˙ . Furthermore, the total wrench acting on Vi Vi link i is the sum of the wrench i transmitted through joint i and the wrench applied to the link through jointFi + 1 (or, for link n, the wrench applied to the link by the environment at the end-effector frame n + 1 ), expressed in the frame i. Therefore, we have the equality { } ˙ T T i i ad ( i i) = i AdT ( i+1); (8.48) G V − Vi G V F − i+1,i F see Figure 8.6. Solving from the tip toward the base, at each link i we solve for the only unknown in Equation (8.48): . Since joint i has only one degree Fi of freedom, five dimensions of the six-vector i are provided “for free” by the structure of the joint, and the actuator onlyF has to provide the scalar force or torque in the direction of the joint’s screw axis:

τ = T . (8.49) i Fi Ai Equation (8.49) provides the torques required at each joint, solving the inverse dynamics problem.

May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org Newton-Euler Inverse Dynamics Algorithm Initialization: • = 0 and ˙ = (ω ˙ , v˙ ) = (0, g) V0 V0 0 0 − - Setting v˙0 = −g is a trick to handle gravity; otherwise one needs to consider gravity for each link during the backward wrench propagation

• n+1: given a priori which is the wrench applied by the environment expressedF in the end-eﬀector frame Forward Iterations: Given θ, θ,˙ θ¨, for i = 1 n do: → −[A ]θ Ti,i−1(θi) = e i i Mi,i−1 (3) h i ˙ Vi = AdTi,i 1 Vi−1 + Aiθi (4) − h i ˙ ˙ ¨ Vi = AdTi,i 1 Vi−1 + adVi Aiθi + Aiθi (5) −

Backward Iterations: For i = n to 1, do T h i ˙ T Fi = AdTi+1,i Fi+1 + GiVi − adVi (GiVi) (6) T τi = Fi Ai (7)

Return: Joint torque vector τ Rn ∈ Newton-Euler Inverse Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 16 / 22 Outline

Introduction •

Newton-Euler Inverse Dynamics of Open Chains •

Forward Dynamics of Open Chains •

Forward Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 17 / 22 Forward Dynamics Problem

• Overall dynamics equation (with nonzero wrench applied by the end-eﬀector):

M(θ)θ¨ = τ(t) h(θ, θ˙) J T (θ) (8) − − Ftip • Forward dynamics: Given θ, θ,˙ τ and , compute the acceleration θ¨ Ftip

• Jacobian J(θ) can be obtained relatively easily.

• However, M(θ) and h(θ, θ˙) can be hard to derive analytically

• We can use inverse dynamics algorithm to numerically compute M(θ) and h(θ, θ˙) for any given joint state θ, θ˙

Forward Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 18 / 22 Computations of M(θ) and h(θ, θ˙)

• Denote our inverse dynamics algorithm: τ = InverseDyn(θ, θ,˙ θ,¨ ) Ftip • Obviously, τ = h(θ, θ˙) if = 0 and θ¨ = 0 Ftip • Therefore, we can compute h(θ, θ˙) = InverseDyn(θ, θ,˙ 0, 0)

• Recall the form of h(θ, θ˙) = C(θ, θ˙)θ˙ + g(θ).

- If ignore external forces, i.g. set g = 0 and Ftip = 0, then g(θ) = 0

- If we further choose θ˙ = 0, then h(θ, θ˙) = 0 ⇒ τ = M(θ)θ¨

• We can compute the jth column of M(θ) by calling the inverse algorithm

¨0 M:,j(θ) = InverseDyn(θ, 0, θj , 0)

¨0 where θj is a vector with all zeros except for a 1 at the jth entry.

Forward Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 19 / 22 Forward Dynamics Algorithm

• Given θ, θ˙, the last slide shows how to compute M(θ) and h(θ, θ˙)

• Now we assume we have θ, θ,˙ τ, M(θ), h(θ, θ˙), , then we can immediately Ftip compute θ¨ using (8).

• Denote this algorithm as θ¨ = ForwardDyn(θ, θ,˙ τ, ) Ftip

• This provides a 2nd-order diﬀerential equation in Rn, we can easily simulate the joint trajectory over any time period (under given initial condition θo and θ˙o)

Forward Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 20 / 22 Examples

MATLAB Examples

Forward Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 21 / 22 More Discussions

•

Forward Dynamics Lecture 13 (ECE5463 Sp18) Wei Zhang(OSU) 22 / 22