Multi-body system dynamics Fa z Ben Amar amar@isir.upmc.fr ... - - PowerPoint PPT Presentation

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Multi-body system dynamics

Fa¨ ız Ben Amar amar@isir.upmc.fr ... 2014

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

1 Introduction 2 Mathematics and Notation 3 Rotation parameters of a rigid body 4 Kinematics of rigid body 5 Kinetics of multibody system 6 Lagrange dynamics 7 Multibody system dynamics 8 Serial manipulator dynamics 9 Dynamic simulation

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Dynamics modeling and simulation

Formulate the system of equations that govern the time evolution of a multibody system under the action of applied (external) forces These are differential-algebraic equations called Equations of Motion (EOM) Understand how to handle various types of applied forces and properly include them in the EOM Understand how to compute reaction forces in any joint connecting any two bodies in the mechanism Study of equilibrium configurations, analyse their stability, linear time-frequence response Understand under what conditions a solution to the EOM exists Numerically solve the resulting (differential-algebraic) EOM

• Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Parametrization

Strict or minimal (can be not representative !!) Joint (Redundant) Cartesian or absolute (highly redundant) EOM are dependent on parameters Each differential equations is associated to one parameter

• φ
• α2
• α4

α1

• α3
• θ1
• θ3
• θ2
• Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Parametrization (2)

Type of parametrization Strict Joint Cartesian Number of parameters Minimal Medium Important Number of differential equations Minimal Medium Important Number of algebraic equation zero Medium Important Non-linearity order High Medium Low EOM obtention Hard Quite hard Easy Computational efficiency Efficient Quite efficient Not efficient Genericity of the software Difficult Quite difficult Easy

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Typical problem in CAD of multibody systems

Kinematic problem (assembly problem) Velocity and acceleration analysis Forward dynamics Inverse dynamics Static analysis of equilibrium Stability of equilibrium and linear analysis

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Forward and Inverse dynamics

Forward dynamics

• )

( ) ( ) ( t q t q t q

• )

(t τ

Useful for simulation Connected with differential equation solver Inverse dynamics

• )

( ) ( ) ( t q t q t q

• )

(t τ

Useful for control Real time constraint and efficiency issue

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Vector and Matrix

Geometric vector

• a is a geometric vector such that

a = ax ex + ay ey + az ez Algebraic vector or matrix column a =   ax ay az   = [ax ay az]T Matrix A = [aij] =      a11 a12 . . . a1n a21 a22 . . . a2n . . . . . . ... . . . am1 am2 . . . amn     

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Dot and cross-product

• Vector form :
• a.

b = ab cos ( a, b) = axbx + ayby + azbz

• a×

b = ab sin ( a, b) u = (aybz−azby) ex+(azbx−axbz) ey+(axby−aybx) ez

• Matrix form :
• a.

b = aT b

• a ×

b = ˜ ab ˜ a =   −az ay az −ax −ay ax   is the cross-product matrix.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Time derivative of scalar function

Derivative w.r.t time of scalar function f = f(q1(t), . . . , qn(t), t) dependent on parameters q(t) and time t d f dt = ∂f ∂q1 ∂f ∂q2 . . . ∂f ∂qn

• 

      

dq1 dt dq2 dt

. . .

dqn dt

        + ∂f ∂t = ∂f ∂q dq dt + ∂f ∂t = fq ˙ q + ft with fq = ∂f ∂q = ∂f ∂q1 ∂f ∂q2 . . . ∂f ∂qn

• Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Time derivative of vectorial function

For a vectorial function f = [f1, f2, . . . , fm](q1, q2, . . . , qn, t), its derivative w.r.t. time df dt = ∂f ∂q dq dt + ∂f ∂t = fq ˙ q + ft with fq the Jacobian of f fq = ∂f ∂q =          

∂f1 ∂q1 ∂f1 ∂q2

. . .

∂f1 ∂qn ∂f2 ∂q1 ∂f2 ∂q2

. . .

∂f2 ∂qn

. . . . . . ... . . .

∂fm ∂q1 ∂fm ∂q2

. . .

∂fm ∂qn

         

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Time derivative of geometric vector

Consider a general vector u How does it move in Space/Body coordinates ? What is its time derivative d

u dt ?

Movement d u depends on observer frames (space or body) (d u)S = (d u)B + (d u)rot = (d u)B + d α × u d u dt

• S

= d u dt

• B

+ ω(B/S) × u

• ω(B/S) is angular velocity of body frame w.r.t. space frame.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Coordinates transformation

Let (x, y, z) and (ξ, η, ζ) 2 frames attached to ground and a body. Let u such that

• u

= ux ex + uy ey + uz ez

• u

= uξ eξ + uη eη + uζ eζ

η ζ

• ξ
• We denote by u, ¯

u coordinates of u in the ground and body frames u = [ux uy uz]T ¯ u = [uξ uη uζ]T u = A¯ u with A = [eξ eη eζ]

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Properties of transformation matrix

A is an rotation matrix because AAT = I3. Then A−1 = AT Inverse transformation ¯ u = AT u 9 parameters of A are called direction cosines Could be used for rotation parameters ... but highly redundant Planar motion s = A¯ s =

• cos φ

− sin φ sin φ cos φ

• ¯

s

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Rigid body coordinates

3 position parameters (rO) 3 orientation parameters (A) rP = rO + uP = rO + A¯ uP

• ξ

η ζ

• Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Rotation matrix

• α

θ

• θ
• η
• θ
• ∆

ζ ξ

• u

=

• I + ˜

v sin θ + (˜ v)2 2 sin2(θ 2)

• ¯

u = A(v, θ)¯ u

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Euler parameters

Quaternion                      p1 = e0 = cos θ 2 p2 = e1 = v1 sin θ 2 p3 = e2 = v2 sin θ 2 p4 = e3 = v3 sin θ 2 Unit quaternion pT p =

3

• k=0

(pk)2 = 1 Euler-Rodriguez formula becomes A = I3 + 2˜ e (e0I3 + ˜ e) where e = [e1, e2, e3]T

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Euler angles Z-X-Z

Composition of three elemental rotations, starting from a known standard orientation rotation by φ about z0-axis, rotation by θ about x1-axis, rotation by ψ about z2-axis

• A =

  cos ψ cos φ − cos θ sin φ sin ψ − sin ψ cos φ − cos θ sin φ cos ψ sin θ sin φ cos ψ sin φ + cos θ cos φ sin ψ − sin ψ sin φ + cos θ cos φ cos ψ − sin θ cos φ sin θ sin ψ sin θ cos ψ cos θ  

Drawback : Singular representation for θ = 0. ψ and φ can not be defined.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Angular velocity of rigid body

uP = A¯ uP

• ξ

η ζ

• ˙

A = ΩA = A ¯ Ω where Ω = ˜ ω, ¯ Ω = ˜ ¯ ω are skew-symmetric matrix corresponding angular velocity ω,¯ ω, which define components of the same vector ω in the space and body frame. ω = A¯ ω ˜ ω = A˜ ¯ ωAT Linearity on time derivative parameters Euler parameters : ω = G(p) ˙ p ¯ ω = ¯ G(p) ˙ p Euler angles : ω = G(θ) ˙ θ ¯ ω = ¯ G(θ) ˙ θ 2D : ω = (0 0 1)T ˙ θ ¯ ω = (0 0 1)T ˙ θ

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Linear velocity and acceleration

Euler parameters : ˙ rP = I −A(p)˜ ¯ uP ¯ G(p) ˙ r ˙ p

• Euler angles :

˙ rP = I −A(θ)˜ ¯ uP ¯ G(θ) ˙ r ˙ θ

• 2D :

˙ rP =

• I

Aθ¯ uP ˙ r ˙ θ

• Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Joint constraint : planar hinge joint

Points P i and P j are always attached ri

P = rj P

Φ(qi, qj) = ri + Ai¯ ui

P − rj − Aj ¯

uj

P = 0

ri

x + ¯

xi

P cos θi − ¯

yi

P sin θi − rj x − ¯

xj

P cos θj + ¯

yj

P sin θj = 0

ri

y + ¯

xi

P sin θi + ¯

yi

P cos θi − rj y − ¯

xj

P sin θj − ¯

yj

P cos θj = 0

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Joint constraint : planar slider joint

Relative orientation remains constant θi − θj − c = 0 A point of j slides along a i axis hiT d = 0 d = ri

P − rj P

= ri + Ai¯ ui

P − rj − Aj ¯

uj

P

hi = ri

P − ri Q

= ri + Ai¯ ui

P − ri − Ai¯

ui

Q

= Ai(¯ ui

P − ¯

ui

Q)

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Joint constraint : spatial joint

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Kinematics of mechanism

• What is kinematics ?

Study of the position, velocity, and acceleration of a system of interconnected bodies that make up a mechanism, independent of the forces that produce the motion

• Problem types

Kinematic Analysis : Interested how components of a certain mechanism move when motion(s) are applied - Compute mobility and hyperstatism

Position Analysis Stage ⇒ challenging Velocity Analysis Stage ⇒ simple Acceleration Analysis Stage ⇒ OK

Kinematic Synthesis : Interested in finding how to design a mechanism to perform a certain operation in a certain way ⇒ highly challenging

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Kinematics of mechanism

Each of the three stages of Kinematics Analysis: position analysis, velocity analysis, and acceleration analysis, follow very similar recipes for finding the position, velocity and acceleration, respectively, of every body in the system To take care of all these stages, ONE step is critical: Write down the constraint equations associated with the joints present in your mechanism Once you have the constraints Φ(q), the rest is quiet All stages crucially rely on the Jacobian matrix Φq : the partial derivative of the constraints w.r.t. the generalized coordinates Velocity and acceleration stages require the solution of linear systems of equations of the form: Φqx = b What is different between the three stages is the expression for the RHS b.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Position analysis

So how do you get the position configuration of the mechanism ? Kinematic Analysis key observation: The number of constraints (kinematic and driving) should be equal to the number of generalized coordinates In other words, NDOF=0 is a prerequisite Φ(q, t) =

• ΦK(q)

ΦD(q, t)

• = 0

This is a nonlinear system with: nc equations nc unknowns that must be solved for q.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Velocity and acceleration analysis

Take one time derivative of constraints Φ(q, t) to obtain the velocity equation: ˙ Φ ≡ d dtΦ(q, t) = 0 ⇒ Φq ˙ q + Φt Φq ˙ q = −Φ(q, t)

• ν−Φt

Take one more time derivative to obtain the acceleration equation: ¨ Φ ≡ d dt ˙ Φ(q, t) = 0 ⇒ Φq¨ q + (Φq ˙ q)q ˙ q + 2Φqt ˙ q + Φtt = 0 Φq¨ q = − (Φq ˙ q)q ˙ q − 2Φqt ˙ q − Φtt

• γ−(Φq ˙

q)q ˙ q−2Φqt ˙ q−Φtt

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Gruebler count of a mechanism

Approximate degree of freedom Gruebler count (number of body parameters minus number of joint constraints) ADOF = dNb −

Nj

• i=1

(d − fi) Nb : Number of moving bodies (i.e. not including the ground) Nj : Number of joints fi : degree of freedom of the ith joint d : problem dimension, d = 3, 6 for planar or spatial mechanism It is easy to show that ADOF =  

Nj

• i=1

fi   − d(Nj − Nb)

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Kinematic degrees of freedom of a mechanism

The exact kinematic degree of freedom depends on how joint axes are located in space. KDOF(q) = dim(q) − rank(ΦK

q (q))

dim(q) = dNb if absolute parameters are used as generalized coordinates. If ΦK

q is full rank matrix, KDOF = ADOF, i.e. there is no redundant

joint constraint, ⇒ the mechanism is isostatic (we are able to determine all joint force components as function of external load). If ΦK

q is not full rank matrix, KDOF > ADOF, i.e. there is

redundant joint constraint, ⇒ the mechanism is hyperstatic (we are not able to determine all joint force components as function of external load, they would depend also on material elasticity). The remained degree of freedom depends on driving constraint. This should be equal to zero for solving velocity equation. RDOF(q) = dim(q) − rank(Φq(q))

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Singular configurations

At a singular configuration (q∗, t∗), the Jacobian is singular i.e. det(Φq(q)) = 0 There is two types of singular configurations Lock-up type : if rank(Jvel) > rank(Φq) Bifurcation type : if rank(Jvel) = rank(Jacc) = rank(Φq), with Jvel [Φq ν] Jacc [Φq γ]

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Mechanism analysis (1)

Use q = [x1, y1, φ1, x2, y2, φ2]T as generalized coordinates

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Mechanism analysis (2)

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Kinetic energy of a rigid body

T = 1 2m˙ rT ˙ r + 1 2ωT Iω as ω = R¯ ω T = 1 2m˙ rT ˙ r + 1 2 ¯ ωT RT IR¯ ω T = 1 2m˙ rT ˙ r + 1 2 ¯ ωT¯ I¯ ω with ¯ I = RT IR or I = R¯ IRT

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Inertia tensor

¯ I =

• V

ρ˜ ¯ u

T P ˜

¯ uP dV

¯ I =          

• V

ρ

• (¯

x2)2 + (¯ x3)2 dV

• V

−ρ¯ x1¯ x2dV

• V

−ρ¯ x1¯ x3dV

• V

ρ

• (¯

x1)2 + (¯ x3)2 dV

• V

−ρ¯ x2¯ x3dV sym

• V

ρ

• (¯

x1)2 + (¯ x2)2 dV          

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Properties of Inertia tensor

Inertia matrix is symmetric and positive-definite Diagonal terms are called moment of inertia Off-diagonal are called products of inertia Inertia matrix is diagonalizable Eigen values are called principal moment of inertia Eigen vectors are called principal axes of inertia Four objects racing down a plane while rolling without slipping. From back to front: spherical shell (red), solid sphere (orange), cylindrical ring (green) and solid cylinder (blue). The time for each object to reach the finishing line depends on their moment of inertia. How wins ? Click here for answer

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Moment of inertia

Moment of inertia is minimal at Center Of Mass (COM) Huyghens formula or parallel axis theorem I = Icom + md2

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Kinetic energy as function of generalized parameters

Linear and angular velocity of link i ˙ ri = vi = Ji

v(θ) ˙

θ ωi = Ji

ω(θ) ˙

θ The total kinetic energy T = 1 2 ˙ θ

T

n

• i=1

miJiT

v Ji v + JiT ω Ri¯

IiRiT Ji

ω

• ˙

θ = 1 2 ˙ θ

T M(θ) ˙

θ = 1 2

• i,j

Mij(θ) ˙ θi ˙ θj

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Mass matrix of a multibody system

M(θ) is the mass (or inertia) matrix, dependent on the configuration θ is square n × n, n is the dimension of θ is symmetric and positive definite has positive eigenvalues 0 < λ1(θ) ≤ . . . ≤ λn(θ) is bounded below and above λmIn×n ≤ M(θ) ≤ λMIn×n and its inverse is also bounded 1 λM In×n ≤ M−1(θ) ≤ 1 λm In×n

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

From Newton to D’Alembert principle

Newton Equation Point mass equation f = m¨ r

• Isaac Newton (1643 - 1727)

D’Alembert principle Virtual work principle f.δr − m¨ r.δr = 0 ∀δr Or the virtual power principle f.˙ r∗ − m¨ r.˙ r∗ = 0 ∀˙ r∗ Jean Le Rond D’Alembert (1717 - 1783)

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Newton-Euler Equations for single body

m¨ r = fe ¯ I ˙ ¯ ω = ¯ ne − ¯ ω × ¯ I¯ ω

• fe, ¯

ne are external force and moment expressed in the body CoM (Center Of Mass) m, I are body mass and its tensor inertia expressed in the CoM. Both equations are theoretically frame independent. The second one is in general projected onto the body frame because it is more practical to express the inertia tensor of a rigid body in its own frame.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Generalized coordinates

A set of parameters q = (q1, q2, . . . , qn) that allows full parametrization of system configuration. q is not necessarily composed by cartesian coordinates. Components could be an angle, a relative distance ... They could have different units. They could be dependent. As any position is described by ri = ri(q1, q2, . . . , qn, t), if qi = qi(t) is obtained then ri = ri(t) could be deduced. ⇒

• r

Double pendulum 2D rep. Surface rep. (closed manifold)

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Holonomic Constraints

Φ(q, t) = 0 Example : Hinge joint

• θ
• θ

Use q = [xi yi θi xj yj θj]T

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Non-Holonomic Constraints

Φ(q, ˙ q, t) = 0 Example : Rolling disk

• φ

θ ψ

• Use q = [x y z φ θ ψ]T

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Other constraints

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Virtual displacement

A virtual displacement is defined as an infinitesimal displacement which is compatible with kinematic constraints. A virtual displacement is imaginary in the sense that it is supposed to happen during that time is kept fixed. r = r(q, t) dr = ∂r ∂qdq + ∂r ∂t dt (1) δr = ∂r ∂q1 δq1 + ∂r ∂q2 δq2 + . . . + ∂r ∂qn δqn =

n

• j=1

∂r ∂qj δqj If the constraints are scleronomic δr ≡ dr, while if they are rheonomic δr = dr.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Generalized force

Forces are not necessarily represented in Cartesian Coordinates but in Generalized coordinates. How we compute Generalized force ? ⇒ Write the virtual work Assume a set of np force f i applied on a system composed by mass points

• i. The virtual work of these forces during a virtual displacement δri

δW =

np

• i=1

f i.δri δri = ∂ri ∂q1 δq1 + ∂ri ∂q2 δq2 + . . . + ∂ri ∂qn δqn =

n

• j=1

∂ri ∂qj δqj = ∂ri ∂q δq δW =

n

• j=1

Qjδqj Qj =

np

• i=1

f i. ∂ri ∂qj =

np

• i=1

f i.ri

qj

Q = [Q1 Q2 . . . Qn]T are called Generalized force of Cartesian forces f i

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Generalized force

in frictionless joint inter-particles in rigid body

• f a spring-damper-actuator

element

• Application to a mechanism :

compute input-output force relationship

θ

• λ

θ

• τ

τ τ τ

• Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Generalized acceleration

Assume a set of np force f i applied on a system composed by mass points i. Generalized acceleration can be defined, similarly to generalized force, by expressing the virtual work of acceleration quantities m¨ r δWa =

np

• i=1

mi¨ ri.δri δWa =

n

• j=1

ajδqj aj =

np

• i=1

mi¨

• ri. ∂ri

∂qj a = [a1 a2 . . . an]T is vector of generalized acceleration.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Generalized acceleration (cont)

We show that aj = d dt ∂T ∂ ˙ qj

• − ∂T

∂qj δWa =

n

• j=1

d dt ∂T ∂ ˙ qj

• − ∂T

∂qj

• δqj =

n

• j=1

ajδqj with T the total kinetic energy (here of a system composed by np mass points. T =

np

• i=1

T i =

np

• i=1

1 2mi˙ riT ˙ ri

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Generalized acceleration (cont)

We use for that d dt ∂ri ∂qj

• =

n

• k=1

∂ ∂qk ∂ri ∂qj

• ˙

qk

• + ∂

∂t ∂ri ∂qj

• =

d dt ∂ri ∂qj

• =

n

• k=1

∂2ri ∂qj∂qk ˙ qk

• + ∂2ri

∂qj∂t = ∂ ∂qj n

• k=1

∂ri ∂qk ˙ qk

• + ∂ri

∂t

• =

∂ ˙ ri ∂qj and ∂ ˙ ri ∂ ˙ qj = ∂ri ∂qj

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Lagrange equation for independent parameters

Recall D’Alembert principle

np

• i=1

(f i − mi¨ ri).δri = 0 The D’Alembert-Lagrange is then given by

n

• j=1

d dt ∂T ∂ ˙ qj

• − ∂T

∂qj − Qj

• δqj = 0

(2) If qj are independent, then we obtain the Lagrange equation d dt ∂T ∂ ˙ qj

• − ∂T

∂qj − Qj = 0 for j = 1, 2, . . . , n

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Lagrangian function

Forces applied to a system are composed by Conservative forces (gravity, spring, ...) that satisfy Qco = − ∂V ∂q T V is a scalar function called potential energy No-conservative forces (actuator, damper ...) Then Q = Qco + Qnc. Let define the Lagrangien L L(q, ˙ q) = T(q, ˙ q) − V (q) Lagrange equation becomes d dt ∂L ∂ ˙ q T − ∂L ∂q T = Qnc

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Potential energy

Gravity potential energy of body i V i

g = migT ri

g is the gravity acceleration ri the position of the c.o.m. Spring (elastic) potential energy Ve = 1 2k(δl)2 k is the spring stiffness δl = l − l0 is the length variation l0 is the rest length l =|| rB − rA || l =

• (rB − rA)T (rB − rA)

1/2 Gravity energy of a multibody system Vg =

n

• i=1

V i

g

• Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Lagrangian equation for constrained parameters

Let q = (q1, q2, . . . , qn)T a set of n parameters, which are constrained by nc algebraic equation Φ(q, t) =     Φ1(q, t) Φ2(q, t) . . . Φnc(q, t)     = 0 A virtual variation δq of generalized parameters must satisfy Φqδq = 0

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Lagrangian equation for constrained parameters (cont)

d dt ∂L ∂ ˙ q T − ∂L ∂q T − Qnc + ΦT

q λ = 0

λ is Lagrange multiplier vector (of dimension nc). Φq is the constraint Jacobian matrix Φq = ∂Φ ∂q =       

∂Φ1 ∂q1 ∂Φ1 ∂q2

. . .

∂Φ1 ∂qn ∂Φ2 ∂q1 ∂Φ2 ∂q2

. . .

∂Φ2 ∂qn

. . . . . . ... . . .

∂Φnc ∂q1 ∂Φnc ∂q2

. . .

∂Φnc ∂qn

       This matrix is of dimension nc × n. It is a full rank matrix if constraints are independent.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Differential-Algebraic Equation (DAE)

     M¨ q + ˙ M ˙ q − ∂T ∂q + ΦT

q λ = Qco + Qnc

Φ(q, t) = 0 After taking two times the derivative with respect to time of constraint equation Φq ˙ q = −∂Φ ∂t = −Φt Φq¨ q = −Φtt − (Φq ˙ q)q ˙ q − 2Φqt ˙ q = γ

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Differential-Algebraic Equation (cont)

• M

ΦT

q

Φq ¨ q λ

• =

Qco + Qnc + Qv γ

• Qv is so-called the quadratic velocity vector

Qv = − ˙ M ˙ q + ∂T ∂q

• M

ΦT

q

Φq

• is a square invertible (if constraints are independent)

matrix of dimension n + nc, then acceleration and Lagrange multiplier can be deduced by solving the linear system Ax = b.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Parameter partitioning and Reduction

Generalized parameter vector can be partitioned into two sets : a dependent set qd and an independent one qi q =

• qT

d qT i

T Φqdδqd + Φqiδqi = 0 Independent parameters must be choosed such that the square matrix Φqd of dimension nc × nc is invertible δqd = −Φ−1

qd Φqiδqi = Φdiδqi

δq = δqd δqi

• =
• Φdi

In−nc

• δqi = Biδqi

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Parameter Partitioning and Reduction (cont)

Bi is a reduction matrix of dimension n × (n − nc) : Bi =

• Φdi

In−nc

• We can show easily that columns of Bi are in the null space of the

constraint Jacobian i.e. ΦqBi = 0 Multiplying the Lagrange equation by BT

i allows to eliminate the

Lagrange multipliers BT

i

• d

dt ∂T ∂ ˙ q T − ∂T ∂q T − Q

• = 0

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Serial manipulator dynamics

The total kinetic energy T = 1 2 ˙ θ

T

n

• i=1

miJiT

v Ji v + JiT ω Ri¯

IiRiT Ji

ω

• ˙

θ = 1 2 ˙ θ

T M(θ) ˙

θ = 1 2

• i,j

Mij(θ) ˙ θi ˙ θj The Lagrangian of the system L(θ, ˙ θ) = T(θ, ˙ θ) − U(θ) = 1 2

• i,j

Mij ˙ θi ˙ θj − U(θ) Applying the Euler-Lagrange equation for the generalized θk parameter and after some algebraic manipulation, we obtain

• j

Mkj ¨ θj +

• i,j

cijk ˙ θi ˙ θj + gk(θ) = τk k = 1, ..., n

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

with cijk := 1

2

• ∂Mkj

∂θi

+ ∂Mki

∂θj − ∂Mij ∂qk

• is the Christoffel symbol.

gk = ∂U

∂qk is the vector due to gravity

τk is actuator torque of joint k (non-conservative generalized force). Robot dynamics equation M(θ)¨ θ + C(θ, ˙ θ) ˙ θ + g(θ) = τ where Ckj := n

i=1 cijk(θ) ˙

qi = 1

2

n

i=1

• ∂Mkj

∂θi

+ ∂Mki

∂θj − ∂Mij ∂qk

• ˙

qi. Property If M is a diagonal constant matrix, each equation of motion is decoupled and takes the form Mkk¨ θk + gk(θ) = τk k = 1...n

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Example (1,2) Using Lagrange equations, express motion equation of Two dof cartesien manipulator Two dof planar robot with 2 rotoide joints

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Example (3) Using Lagrange equations, express motion equation of Two dof planar robot with rotoide joint and remote actuators.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Example (4) Using Lagrange equations, express motion equation of Five bar linkage and parallelogram based 2 dof arm.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Skew-symmetry, passivity and energy conservation

Property S(θ, ˙ θ) = ˙ M − 2C ∈ ℜn×n is a skew-symmetric matrix This property is also known as the passivity or energy conservation energy property, used for the proof of many control laws. It says that in the absence of friction, the net energy of the robot system is conserved A robot can not create energy

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Linearity in the parameters

M(θ)¨ θ + C(θ, ˙ θ) ˙ θ + g(θ) =: Y(θ, ˙ θ, ¨ θ)π = τ π is a l dimension vector of robot parameters, depending on mass and inertia properties of the robot Y(θ, ˙ θ, ¨ θ) is called the regressor (n × l matrix) The number of parameters is less than 10n (10 is the number of inertia parameters of a rigid body) Representation useful for parametric identification Example Define the parameter vector of the two-revolute joints arm

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Newton-Euler recursive formulation for inverse dynamics

Inputs : θ(t), ˙ θ(t), ¨ θ(t) Outputs : τ(t) Two stages :

1 Forward recursion : compute absolute velocities and accelerations

(linear and angular) for each link starting from the base and going to the end-effector

2 Backward recursion : compute actuator torques, forces and

moments in each joint starting from the wrist (end-effector) and ending in the base

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Forward recursion

If the joint i is of type R (Rotoide) The angular velocity and acceleration of body (i, i = 1 . . . n ) can be computed from that of body (i − 1) ωi = ωi−1 + ˙ θiei ˙ ωi = ˙ ωi−1 + ωi−1 × ˙ θiei + ¨ θiei The position, the linear velocity and the linear acceleration of the c.o.m

• f body i can be computed from that of body (i − 1)

ri = ri−1 − ρi−1 + ai−1 + ρi ˙ ri = ˙ ri−1 + ωi−1 × (ai−1 − ρi−1) + ωi × ρi ¨ ri = ¨ ri−1 + ˙ ωi−1 × (ai−1 − ρi−1) + . . . ωi−1 × (ωi−1 × (ai−1 − ρi−1)) + ˙ ωi × ρi + ωi × (ωi × ρi)

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

If the joint i is of type P (Prismatic) ωi = ωi−1 ˙ ωi = ˙ ωi−1 ri = ri−1 − ρi−1 + di−1 + biei + ρi ˙ ri = ˙ ri−1 + ωi−1 × (di−1 − ρi−1) + ωi × (ρi + biei) + ˙ biei ¨ ri = ¨ ri−1 + ˙ ωi−1 × (di−1 − ρi−1) + ωi−1 × (ωi−1 × (di−1 − ρi−1)) + . . . ˙ ωi × (ρi + biei) + ωi × (ωi × (ρi + biei)) + ¨ biei + ωi × ˙ biei ˙ r0 = 0 ω0 = 0, ˙ ω0 = 0 are angular velocity and acceleration of the base (0) ei is the unit vector of joint i which connect bodies (i − 1) and i Frame independent relations

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Backward recursion

Let define fi, ni force and torque, expressed in the joint center, applied to body i by both body i − 1 and actuator i. Applying Newton-Euler equations to the end-effector, yields to fn = mn¨ rn − f − mng nn = In ˙ ωn + ωn × (Inωn) − n + ρn × fn f, n are force and torque, expressed in the c.o.m of the end-effector., applied by the environment. Frame independent equation

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Applying Newton-Euler equations to body i, yields to fi = mi¨ ri + fi+1 − mig ni = Ii ˙ ωi + ωi × (Iiωi) + ni+1 + . . . (ai − ρi) × fi+1 + ρi × fi Finally, the actuator torque (or force) is given by τi = eT

i ni if joint is of type R

τi = eT

i fi if joint is of type P

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Natural orthogonal complement

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Cartesian dynamics

Remainder : Joint space dynamics M(θ)¨ θ + C(θ, ˙ θ) ˙ θ + g(θ) = τ Assume x = h(θ) any variables of interest i.e. Cartesian or task space position/orientation of the end-effector. Differentiating twice this non-linear function yields to ˙ x = J(θ) ˙ θ ¨ x = J(θ)¨ θ + ˙ J(θ) ˙ θ where J ≡ ∂h ∂θ is the Jacobian of h

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

After some algebraic manipulation, we obtain the cartesian dynamic equation ¯ M¨ x + ¯ C ˙ x + ¯ g = ¯ F where ¯ M = J−tMJ−1 ¯ C = J−T (CJ−1 − MJ−T ˙ JJ−1) ¯ g = J−T g τ = JT F Properties ¯ M is symmetric, definite positive, and bounded ¯ M − 2 ¯ C is a skew-symmetric matrix

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Friction

Main source of friction comes from gears (not perfectly round, presence

• f eccentricity). The lost of energy efficiency due to friction can reach

25-30 %. In general, friction can be included in the dynamic model by adding a dissipative term τ f M(θ)¨ θ + C(θ, ˙ θ) ˙ θ + g(θ) = τ + τ f Friction is a local effect, so τ f is decoupled among the joints. The general form of friction joint torque is composed by viscous and dry components τf = τv + τd = −ν ˙ θ + τd( ˙ θ) Viscous friction τv = −ν ˙ θ ν is a viscous-friction constant

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Dry friction (Coulomb model) τd = −c sgn( ˙ θ) where c is the constant coefficient (could be different for each joint), and sgn is the signum function sgn(x) =    +1 if x > 0 undeterminate if x = 0 −1 if x < 0 Finally : τ

• Actuator torque

= M(θ)¨ θ

Create acceleration

+ C(θ, ˙ θ) ˙ θ

• centrifugical and coriolis

+ g(θ)

• gravity

+ τ f

• friction

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Joint flexibility

Sources of flexibility Gears, Harmonic drives Belt transmission Fluid compressibility of hydraulic actuator Equation of motions : Il¨ ql + Bl ˙ ql + k(ql − qm) = Im¨ qm + Bm ˙ qm − k(ql − qm) = u

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

If we consider the system having ql as output and u as input, we show that the open loop characteristic polynomial is : JlJms4 + (JlBm + JmBl)s3 + (k(Jl + Jm) + BlBm)s2 + k(Bl + Bm)s which could be simplified, for low viscous friction Bm and Bl, into JlJms4 + k(Jl + Jm)s2 The open loop system has a double poles at the origin, and a pair of complex conjugate poles s = ±jω with ω2 = k( 1

Jl + 1 Jm ).

In fact, poles will be shifted to left half plane near the pole of the undamped system.

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Simulation based forward dynamics

Given the actuator torque τ, the joint acceleration can be computed from ¨ θ = M−1(θ)

• τ − C(θ, ˙

θ) ˙ θ − g(θ)

• Given the initial conditions

θ(0) = θ0 ˙ θ(0) = ˙ θ0 We can integrate forward in time by steps of size h. The simplest integration scheme, called Explicit Euler : starting with t = 0, iteratively compute ˙ θ(t + ∆t) = ˙ θ(t) + ¨ θ(t)h θ(t + ∆t) = θ(t) + ˙ θ(t)h + 1 2 ¨ θ(t)h2

Multi-body system dynamics

Outline Introduction Mathematics Rotation Kinematics Kinetics Dynamics Multibody Manipulator Simulation

Euler integration is conceptually simple, but other more sophisticated integration techniques are recommended for accurate and efficient simulation ODE Ordinary Differential Equation ODE ˙ y = f(y, t) Solved iteratively by yi+1 = yi + hg h is a time-step Runge-Kutta 4th order solver (RK4) g = 1 6(f1 + 2f2 + 2f3 + f4) f1 = f(yi, ti) f2 = f(yi + h 2 f1, ti + h 2 ) f3 = f(yi + h 2 f2, ti + h 2 ) f4 = f(yi + hf3, ti + h)

Multi-body system dynamics