Control of industrial robots Robot dynamics

Control of industrial robots

Robot dynamics

Prof. Paolo Rocco ([email protected])Politecnico di Milano, Dipartimento di Elettronica, Informazione e Bioingegneria

Control of industrial robots – Robot dynamics – Paolo Rocco

With these slides we will derive the dynamic model of the manipulator

The dynamic model accounts for the relation between the sources of motion (forces and moments) and the resulting motion (positions and velocities)

u(t)

q(t)

Systematic methods exist to derive the dynamic model of the manipulators, which will be reviewed here, along with the main properties of such model

Introduction


Consider a point with mass m, whose position is described by vector 𝐩𝐩 with respect to a xyz frame.

We define kinetic energy of the point the quantity:

P

x

y

z

O𝑇𝑇 =12𝑚𝑚�̇�𝐩𝑇𝑇�̇�𝐩

Consider now a rigid body, with mass 𝑚𝑚, volume 𝑉𝑉 and density 𝜌𝜌. The kinetic energy is defined with the integral:

𝑇𝑇 =12�𝑉𝑉�̇�𝐩𝑇𝑇�̇�𝐩𝜌𝜌𝜌𝜌𝑉𝑉

P

x

y

z

O

dV

Similarly, for a system of points:

𝑇𝑇 =12�𝑖𝑖=1

𝑛𝑛

𝑚𝑚𝑖𝑖�̇�𝐩𝑖𝑖𝑇𝑇�̇�𝐩𝑖𝑖

Kinetic energy


A system of position forces (i.e. depending only on the positions of the points of application) is said to be conservative is the work made by each force does not depend on the trajectory followed by the point of application, but only on the initial and final positions.In this case the elementary work coincides with the differential, with changed sign, of a function called potential energy:

𝜌𝜌𝑑𝑑 = −𝜌𝜌𝑑𝑑

An example of a system of conservative forces is the gravitational force.For a point mass we have the potential energy:

𝑑𝑑 = −𝑚𝑚𝐠𝐠0𝑇𝑇𝐩𝐩

For a rigid body:

𝑑𝑑 = −�𝑉𝑉𝐠𝐠0𝑇𝑇𝐩𝐩𝜌𝜌𝜌𝜌𝑉𝑉 = −𝑚𝑚𝐠𝐠0𝑇𝑇𝐩𝐩𝑙𝑙

where 𝐠𝐠0 is the gravity acceleration vector.

where 𝐩𝐩𝑙𝑙 is the position of the center of mass.

Potential energy


Let us consider a system of r rigid bodies (as for example, the links of a robot). If all these bodies are free to move in space, the motion of the system is, at each time instant, described by means of 6𝑟𝑟 coordinates 𝐱𝐱.

Suppose now that limitations exist in the motion of the bodies of the system (as for example the presence of a joint, which eliminates five out of the six relative degrees of freedom between two consecutive links).A constraint thus exists on the motion of the bodies, which we will express with the relation:

𝐡𝐡 𝐱𝐱 = 0

Such a constraint is said to be holonomic (it depends only on position coordinates, not velocities) and stationary (it does not change with time).

Systems of rigid bodies


𝐡𝐡 𝐱𝐱 = 0

If the constraints 𝐡𝐡 are composed of 𝑠𝑠 scalar equations and they are all continuously differentiable, it is possible, by means of the constraints, to eliminate 𝑠𝑠 coordinates from the system equations.

The remaining 𝑛𝑛 = 6𝑟𝑟 − 𝑠𝑠 coordinates are called free, or Lagrangian, or natural, or generalized coordinates. 𝑛𝑛 is the number of degrees of freedom of the mechanical system.

For example in a robot with 6 joints, out of the 36 original coordinates, 30 are eliminated by virtue of the constraints imposed by the 6 joints. The remaining 6 are the Lagrangiancoordinates (typically the joint variables used in the kinematic model).

Free coordinates


𝐿𝐿 𝐪𝐪, �̇�𝐪 = 𝑇𝑇 𝐪𝐪, �̇�𝐪 − 𝑑𝑑 𝐪𝐪

𝜌𝜌𝑑𝑑 = �𝑖𝑖=1

𝑛𝑛

𝜉𝜉𝑖𝑖𝜌𝜌𝑞𝑞𝑖𝑖

Given a system of rigid bodies, whose positions and orientations can be expressed by means of 𝑛𝑛generalized coordinates 𝑞𝑞𝑖𝑖, we define Lagrangian of the mechanical system the quantity:

where 𝑇𝑇 and 𝑑𝑑 are the kinetic and the potential energies, respectively. Let then 𝜉𝜉𝑖𝑖 be the generalized forces associated with the generalized coordinates 𝑞𝑞𝑖𝑖. The elementary work performed by the forces acting on the system can be expressed as:

It can be proven that the dynamics of the system of bodies is expressed by the following Lagrange’s equations:

𝜌𝜌𝜌𝜌𝑑𝑑

𝜕𝜕𝐿𝐿𝜕𝜕�̇�𝑞𝑖𝑖

−𝜕𝜕𝐿𝐿𝜕𝜕𝑞𝑞𝑖𝑖

= 𝜉𝜉𝑖𝑖 , 𝑖𝑖 = 1, … ,𝑛𝑛

Lagrange’s equations


Let us consider a system composed of a motor rigidly connected to a load, subjected to the gravitational force. Let: 𝐼𝐼𝑚𝑚 and 𝐼𝐼 the moments of inertia of the motor and the load with

respect to the motor spinning axis 𝑚𝑚 the mass of the load 𝑙𝑙 the distance of the center of mass of the load from the axis of

the motor.

Kinetic energy of the motor: 𝑇𝑇𝑚𝑚 =12𝐼𝐼𝑚𝑚�̇�𝜗2

mg

ϑ

τ

l x

y

C

Kinetic energy of the load: 𝑇𝑇𝑐𝑐 =12𝐼𝐼�̇�𝜗2

An example


Gravitational potential energy:

Lagrangian: 𝐿𝐿 = 𝑇𝑇𝑚𝑚 + 𝑇𝑇𝑐𝑐 − 𝑑𝑑 =12𝐼𝐼𝑚𝑚 + 𝐼𝐼 �̇�𝜗2 − 𝑚𝑚𝑚𝑚𝑚 sin𝜗𝜗

Lagrange’s equations:

mg

ϑ

τ

l𝑑𝑑 = −𝑚𝑚𝐠𝐠0𝑇𝑇𝐩𝐩𝑙𝑙 = −𝑚𝑚 0 −𝑚𝑚 𝑚 cos𝜗𝜗

𝑚 sin𝜗𝜗 = 𝑚𝑚𝑚𝑚𝑚 sin𝜗𝜗

𝜌𝜌𝜌𝜌𝑑𝑑𝜕𝜕𝐿𝐿𝜕𝜕�̇�𝜗

−𝜕𝜕𝐿𝐿𝜕𝜕𝜗𝜗

= 𝜏𝜏 ⇒𝜌𝜌𝜌𝜌𝑑𝑑

𝐼𝐼 + 𝐼𝐼𝑚𝑚 �̇�𝜗 + 𝑚𝑚𝑚𝑚𝑚 cos𝜗𝜗 = 𝜏𝜏

Then:

𝐼𝐼 + 𝐼𝐼𝑚𝑚 �̈�𝜗 + 𝑚𝑚𝑚𝑚𝑚 cos𝜗𝜗 = 𝜏𝜏 This equation can be easily interpreted as the equilibrium of moments around the rotation axis.

An example


The contribution of kinetic energy of a single link can be computed with the following integral:

𝑇𝑇𝑖𝑖 =12�𝑉𝑉𝑖𝑖�̇�𝐩𝑖𝑖∗

𝑇𝑇�̇�𝐩𝑖𝑖∗𝜌𝜌𝜌𝜌𝑉𝑉

generic point along the link𝐩𝐩𝑖𝑖∗

Position of the center of mass: 𝐩𝐩𝑙𝑙𝑖𝑖 =1𝑚𝑚𝑖𝑖

�𝑉𝑉𝑖𝑖𝐩𝐩𝑖𝑖∗𝜌𝜌𝜌𝜌𝑉𝑉

Velocity of the generic point: �̇�𝐩𝑖𝑖∗ = �̇�𝐩𝑙𝑙𝑖𝑖 + ω𝑖𝑖 × 𝐫𝐫𝑖𝑖 = �̇�𝐩𝑙𝑙𝑖𝑖 + 𝐒𝐒 ω𝑖𝑖 𝐫𝐫𝑖𝑖

where: 𝐒𝐒 ω𝑖𝑖 =0 −𝜔𝜔𝑖𝑖𝑖𝑖 𝜔𝜔𝑖𝑖𝑖𝑖𝜔𝜔𝑖𝑖𝑖𝑖 0 −𝜔𝜔𝑖𝑖𝑖𝑖−𝜔𝜔𝑖𝑖𝑖𝑖 𝜔𝜔𝑖𝑖𝑖𝑖 0

(skew-symmetric matrix)

Kinetic energy of a link


Translational contribution: 12�𝑉𝑉𝑖𝑖�̇�𝐩𝑙𝑙𝑖𝑖

𝑇𝑇�̇�𝐩𝑙𝑙𝑖𝑖𝜌𝜌𝜌𝜌𝑉𝑉 =12𝑚𝑚𝑖𝑖�̇�𝐩𝑙𝑙𝑖𝑖

𝑇𝑇�̇�𝐩𝑙𝑙𝑖𝑖

Mutual contribution:

�𝑉𝑉𝑖𝑖�̇�𝐩𝑙𝑙𝑖𝑖

𝑇𝑇𝐒𝐒 ω𝑖𝑖 𝐫𝐫𝑖𝑖𝜌𝜌𝜌𝜌𝑉𝑉 = �̇�𝐩𝑙𝑙𝑖𝑖𝑇𝑇𝐒𝐒 ω𝑖𝑖 �

𝑉𝑉𝑖𝑖𝐩𝐩𝑖𝑖∗ − 𝐩𝐩𝑙𝑙𝑖𝑖 𝜌𝜌𝜌𝜌𝑉𝑉 = 0

Rotational contribution:

12�𝑉𝑉𝑖𝑖

𝐫𝐫𝑖𝑖𝑇𝑇𝐒𝐒𝑇𝑇 ω𝑖𝑖 𝐒𝐒 ω𝑖𝑖 𝐫𝐫𝑖𝑖𝜌𝜌𝜌𝜌𝑉𝑉 =12ω𝑖𝑖

𝑇𝑇 �𝑉𝑉𝑖𝑖𝐒𝐒𝑇𝑇 𝐫𝐫𝑖𝑖 𝐒𝐒 𝐫𝐫𝑖𝑖 𝜌𝜌𝜌𝜌𝑉𝑉 ω𝑖𝑖 =

12ω𝑖𝑖

𝑇𝑇𝐈𝐈𝑖𝑖ω𝑖𝑖𝐒𝐒 ω𝑖𝑖 𝐫𝐫𝑖𝑖 = −𝐒𝐒 𝐫𝐫𝑖𝑖 ω𝑖𝑖

Note:

Then: 𝑇𝑇𝑖𝑖 =12𝑚𝑚𝑖𝑖�̇�𝐩𝑙𝑙𝑖𝑖

𝑇𝑇�̇�𝐩𝑙𝑙𝑖𝑖 +12ω𝑖𝑖𝑇𝑇𝐈𝐈𝑖𝑖ω𝑖𝑖

velocity of center of mass

angular velocity of the link

König Theorem

Kinetic energy of a link


ω𝑖𝑖𝑖𝑖 = 𝐑𝐑𝑖𝑖𝑇𝑇ω𝑖𝑖

(symmetric matrix)

𝐈𝐈𝑖𝑖 =

� 𝑟𝑟𝑖𝑖𝑖𝑖2 + 𝑟𝑟𝑖𝑖𝑖𝑖2 𝜌𝜌𝜌𝜌𝑉𝑉 −�𝑟𝑟𝑖𝑖𝑖𝑖𝑟𝑟𝑖𝑖𝑖𝑖𝜌𝜌𝜌𝜌𝑉𝑉 −�𝑟𝑟𝑖𝑖𝑖𝑖𝑟𝑟𝑖𝑖𝑖𝑖𝜌𝜌𝜌𝜌𝑉𝑉

∗ � 𝑟𝑟𝑖𝑖𝑖𝑖2 + 𝑟𝑟𝑖𝑖𝑖𝑖2 𝜌𝜌𝜌𝜌𝑉𝑉 −�𝑟𝑟𝑖𝑖𝑖𝑖𝑟𝑟𝑖𝑖𝑖𝑖𝜌𝜌𝜌𝜌𝑉𝑉

∗ ∗ � 𝑟𝑟𝑖𝑖𝑖𝑖2 + 𝑟𝑟𝑖𝑖𝑖𝑖2 𝜌𝜌𝜌𝜌𝑉𝑉

=𝐼𝐼𝑖𝑖𝑖𝑖𝑖𝑖 −𝐼𝐼𝑖𝑖𝑖𝑖𝑖𝑖 −𝐼𝐼𝑖𝑖𝑖𝑖𝑖𝑖∗ 𝐼𝐼𝑖𝑖𝑖𝑖𝑖𝑖 −𝐼𝐼𝑖𝑖𝑖𝑖𝑖𝑖∗ ∗ 𝐼𝐼𝑖𝑖𝑖𝑖𝑖𝑖

The inertia tensor, if expressed in the base frame, depends on the robot configuration. If the angular velocity is expressed with reference to a frame rigidly attached to the link (for example the DH frame):

the inertia tensor referred to this frame is a constant matrix. Moreover:

𝐈𝐈𝑖𝑖 = 𝐑𝐑𝑖𝑖𝐈𝐈𝑖𝑖𝑖𝑖𝐑𝐑𝑖𝑖𝑇𝑇

We define inertia tensor the matrix:

Inertia tensor


Let us sum the translational and rotational contributions:

𝑇𝑇𝑖𝑖 =12𝑚𝑚𝑖𝑖�̇�𝐩𝑙𝑙𝑖𝑖

𝑇𝑇�̇�𝐩𝑙𝑙𝑖𝑖 +12ω𝑖𝑖𝑇𝑇𝐑𝐑𝑖𝑖𝐈𝐈𝑖𝑖𝑖𝑖𝐑𝐑𝑖𝑖𝑇𝑇ω𝑖𝑖

Linear velocity:

�̇�𝐩𝑙𝑙𝑖𝑖 = 𝐣𝐣𝑃𝑃1𝑙𝑙𝑖𝑖 �̇�𝑞1 + ⋯+ 𝐣𝐣𝑃𝑃𝑖𝑖

𝑙𝑙𝑖𝑖 �̇�𝑞𝑖𝑖 = 𝐉𝐉𝑃𝑃𝑙𝑙𝑖𝑖 �̇�𝐪 𝐉𝐉𝑃𝑃

𝑙𝑙𝑖𝑖 = 𝐣𝐣𝑃𝑃1𝑙𝑙𝑖𝑖 ⋯ 𝐣𝐣𝑃𝑃𝑖𝑖

𝑙𝑙𝑖𝑖 0 ⋯ 0

Angular velocity:

ω𝑖𝑖 = 𝐣𝐣𝑂𝑂1𝑙𝑙𝑖𝑖 �̇�𝑞1 + ⋯+ 𝐣𝐣𝑂𝑂𝑖𝑖

𝑙𝑙𝑖𝑖 �̇�𝑞𝑖𝑖 = 𝐉𝐉𝑂𝑂𝑙𝑙𝑖𝑖 �̇�𝐪 𝐉𝐉𝑂𝑂

𝑙𝑙𝑖𝑖 = 𝐣𝐣𝑂𝑂1𝑙𝑙𝑖𝑖 ⋯ 𝐣𝐣𝑂𝑂𝑖𝑖

𝑙𝑙𝑖𝑖 0 ⋯ 0

Sum of the contributions


𝐣𝐣𝑃𝑃𝑃𝑃𝑙𝑙𝑖𝑖

𝐣𝐣𝑂𝑂𝑃𝑃𝑙𝑙𝑖𝑖

=

𝐳𝐳𝑃𝑃−10 prismatic joint

𝐳𝐳𝑃𝑃−1 × 𝐩𝐩𝑙𝑙𝑖𝑖 − 𝐩𝐩𝑃𝑃−1𝐳𝐳𝑃𝑃−1

rotationa𝑚 joint

The Jacobians are computed column by column, accounting for the effect of the related joint velocity on the end effector linear and angular velocities.

Prismatic jointo Contribution of linear velocity along axis 𝐳𝐳𝑃𝑃−1o No contribution of angular velocity

Rotational jointo Contribution of linear velocity at point 𝐩𝐩𝑙𝑙𝑖𝑖 induced by a rotation around axis 𝐳𝐳𝑃𝑃−1o Contribution of angular velocity around axis 𝐳𝐳𝑃𝑃−1

Computation of the Jacobians


Substituting expressions for the linear and angular velocities:

𝑇𝑇𝑖𝑖 =12𝑚𝑚𝑖𝑖�̇�𝐪𝑇𝑇𝐉𝐉𝑃𝑃

𝑙𝑙𝑖𝑖 𝑇𝑇𝐉𝐉𝑃𝑃𝑙𝑙𝑖𝑖 �̇�𝐪 +

12�̇�𝐪𝑇𝑇𝐉𝐉𝑂𝑂

𝑙𝑙𝑖𝑖 𝑇𝑇𝐑𝐑𝑖𝑖𝐈𝐈𝑖𝑖𝑖𝑖𝐑𝐑𝑖𝑖𝑇𝑇𝐉𝐉𝑂𝑂𝑙𝑙𝑖𝑖 �̇�𝐪

By summing the contributions of all the links we obtain the kinetic energy of the whole arm:

𝑇𝑇 =12�𝑖𝑖=1

𝑛𝑛

�𝑃𝑃=1

𝑛𝑛

𝑏𝑏𝑖𝑖𝑃𝑃 𝐪𝐪 �̇�𝑞𝑖𝑖�̇�𝑞𝑃𝑃 =12�̇�𝐪𝑇𝑇𝐁𝐁 𝐪𝐪 �̇�𝐪

where:

𝐁𝐁 𝐪𝐪 = �𝑖𝑖=1

𝑛𝑛

𝑚𝑚𝑖𝑖𝐉𝐉𝑃𝑃𝑙𝑙𝑖𝑖 𝑇𝑇𝐉𝐉𝑃𝑃

𝑙𝑙𝑖𝑖 + 𝐉𝐉𝑂𝑂𝑙𝑙𝑖𝑖 𝑇𝑇𝐑𝐑𝑖𝑖𝐈𝐈𝑖𝑖𝑖𝑖𝐑𝐑𝑖𝑖𝑇𝑇𝐉𝐉𝑂𝑂

𝑙𝑙𝑖𝑖

is the inertia matrix of the manipulator.

�symmetric

positive definite

depends on 𝐪𝐪

Inertia matrix


The potential energy of a rigid link is related just to the gravitational force:

𝑑𝑑𝑖𝑖 = −�𝑉𝑉𝐠𝐠0𝑇𝑇𝐩𝐩𝑖𝑖∗𝜌𝜌𝜌𝜌𝑉𝑉 = −𝑚𝑚𝑖𝑖𝐠𝐠0𝑇𝑇𝐩𝐩𝑙𝑙𝑖𝑖

where 𝐠𝐠0 is the gravity acceleration vector expressed in the base frame.

The potential energy of the whole manipulator is then the sum of the single contributions:

𝑑𝑑 = �𝑖𝑖=1

𝑛𝑛

𝑑𝑑𝑖𝑖 = −�𝑖𝑖=1

𝑛𝑛

𝑚𝑚𝑖𝑖𝐠𝐠0𝑇𝑇𝐩𝐩𝑙𝑙𝑖𝑖

Potential energy


The Lagrangian of the manipulator is:

𝐿𝐿 𝐪𝐪, �̇�𝐪 = 𝑇𝑇 𝐪𝐪, �̇�𝐪 − 𝑑𝑑 𝐪𝐪 =12�𝑖𝑖=1

𝑛𝑛

�𝑃𝑃=1

𝑛𝑛

𝑏𝑏𝑖𝑖𝑃𝑃 𝐪𝐪 �̇�𝑞𝑖𝑖�̇�𝑞𝑃𝑃 + �𝑖𝑖=1

𝑛𝑛

𝑚𝑚𝑖𝑖𝐠𝐠0𝑇𝑇𝐩𝐩𝑙𝑙𝑖𝑖 𝐪𝐪

If we differentiate the Lagrangian:

Furthermore:

𝜕𝜕𝑑𝑑𝜕𝜕𝑞𝑞𝑖𝑖

= −�𝑃𝑃=1

𝑛𝑛

𝑚𝑚𝑃𝑃𝐠𝐠0𝑇𝑇𝜕𝜕𝐩𝐩𝑙𝑙𝑃𝑃𝜕𝜕𝑞𝑞𝑖𝑖

= −�𝑃𝑃=1

𝑛𝑛

𝑚𝑚𝑃𝑃𝐠𝐠0𝑇𝑇𝐣𝐣𝑃𝑃𝑖𝑖𝑙𝑙𝑗𝑗 𝐪𝐪 = 𝑚𝑚𝑖𝑖 𝐪𝐪

𝜌𝜌𝜌𝜌𝑑𝑑

𝜕𝜕𝐿𝐿𝜕𝜕�̇�𝑞𝑖𝑖

=𝜌𝜌𝜌𝜌𝑑𝑑

𝜕𝜕𝑇𝑇𝜕𝜕�̇�𝑞𝑖𝑖

=𝜌𝜌𝜌𝜌𝑑𝑑

�𝑃𝑃=1

𝑛𝑛

𝑏𝑏𝑖𝑖𝑃𝑃 𝐪𝐪 �̇�𝑞𝑃𝑃 = �𝑃𝑃=1

𝑛𝑛

𝑏𝑏𝑖𝑖𝑃𝑃 𝐪𝐪 �̈�𝑞𝑃𝑃 + �𝑃𝑃=1

𝑛𝑛𝜌𝜌𝑏𝑏𝑖𝑖𝑃𝑃 𝐪𝐪𝜌𝜌𝑑𝑑 �̇�𝑞𝑃𝑃 =

= �𝑃𝑃=1

𝑛𝑛


𝑛𝑛

�𝑘𝑘=1

𝑛𝑛𝜕𝜕𝑏𝑏𝑖𝑖𝑃𝑃 𝐪𝐪𝜕𝜕𝑞𝑞𝑘𝑘

�̇�𝑞𝑘𝑘 �̇�𝑞𝑃𝑃

𝜕𝜕𝑇𝑇𝜕𝜕𝑞𝑞𝑖𝑖

=12�𝑃𝑃=1

𝑛𝑛

�𝑘𝑘=1

𝑛𝑛𝜕𝜕𝑏𝑏𝑃𝑃𝑘𝑘 𝐪𝐪𝜕𝜕𝑞𝑞𝑖𝑖

�̇�𝑞𝑘𝑘�̇�𝑞𝑃𝑃

Equations of motion


From Lagrange’s equations we obtain:

where:

�𝑃𝑃=1

𝑛𝑛


𝑛𝑛

�𝑘𝑘=1

𝑛𝑛

ℎ𝑖𝑖𝑃𝑃𝑘𝑘 𝐪𝐪 �̇�𝑞𝑘𝑘�̇�𝑞𝑃𝑃 + 𝑚𝑚𝑖𝑖 𝐪𝐪 = 𝜉𝜉𝑖𝑖 𝑖𝑖 = 1, … ,𝑛𝑛

ℎ𝑖𝑖𝑃𝑃𝑘𝑘 =𝜕𝜕𝑏𝑏𝑖𝑖𝑃𝑃𝜕𝜕𝑞𝑞𝑘𝑘

−12𝜕𝜕𝑏𝑏𝑃𝑃𝑘𝑘𝜕𝜕𝑞𝑞𝑖𝑖

Acceleration terms:

𝑏𝑏𝑖𝑖𝑖𝑖 : inertia moment as “seen” from the axis of joint 𝑖𝑖

𝑏𝑏𝑖𝑖𝑃𝑃 : effect of the acceleration of joint 𝑗𝑗 on the joint 𝑖𝑖

Centrifugal and Coriolis terms:

ℎ𝑖𝑖𝑃𝑃𝑃𝑃�̇�𝑞𝑃𝑃2 : centrifugal effect induced at joint 𝑖𝑖 by the velocity of joint 𝑗𝑗

ℎ𝑖𝑖𝑃𝑃𝑘𝑘�̇�𝑞𝑃𝑃�̇�𝑞𝑘𝑘: Coriolis effect induced at joint 𝑖𝑖 by the vel. of joints 𝑗𝑗 and 𝑘𝑘

gravitational term, depends only on the joint positions

ℎ𝑖𝑖𝑖𝑖𝑖𝑖 = 0

�

Equations of motion


Besides the gravitational conservative forces, other forces act on the manipulator: actuation torques

viscous friction torques

static friction torques

τ

−𝐅𝐅𝑣𝑣�̇�𝐪

−𝐟𝐟𝑠𝑠 𝐪𝐪, �̇�𝐪

diagonal matrix of viscous friction coefficients𝐅𝐅𝑣𝑣

𝐟𝐟𝑠𝑠 𝐪𝐪, �̇�𝐪 function that models the static friction at the joint

Non conservative forces


In vector form the dynamic model can be expressed as follows:

𝐁𝐁 𝐪𝐪 �̈�𝐪 + 𝐂𝐂 𝐪𝐪, �̇�𝐪 �̇�𝐪 + 𝐅𝐅𝑣𝑣�̇�𝐪 + 𝐟𝐟𝑠𝑠 𝐪𝐪, �̇�𝐪 + 𝐠𝐠 𝐪𝐪 = τ

where 𝐂𝐂 is a suitable 𝑛𝑛 × 𝑛𝑛 matrix, whose elements satisfy the equation:

�𝑃𝑃=1

𝑛𝑛

𝑐𝑐𝑖𝑖𝑃𝑃�̇�𝑞𝑃𝑃 = �𝑃𝑃=1

𝑛𝑛

�𝑘𝑘=1

𝑛𝑛

ℎ𝑖𝑖𝑃𝑃𝑘𝑘�̇�𝑞𝑘𝑘�̇�𝑞𝑃𝑃 𝐂𝐂 is not symmetric in general

Complete dynamic model


The choice of matrix 𝐂𝐂 is not unique. One possible choice is the following one:

�𝑃𝑃=1

𝑛𝑛

𝑐𝑐𝑖𝑖𝑃𝑃�̇�𝑞𝑃𝑃 = �𝑃𝑃=1

𝑛𝑛

�𝑘𝑘=1

𝑛𝑛

ℎ𝑖𝑖𝑃𝑃𝑘𝑘�̇�𝑞𝑘𝑘�̇�𝑞𝑃𝑃 = �𝑃𝑃=1

𝑛𝑛

�𝑘𝑘=1

𝑛𝑛𝜕𝜕𝑏𝑏𝑖𝑖𝑃𝑃𝜕𝜕𝑞𝑞𝑘𝑘

−12𝜕𝜕𝑏𝑏𝑃𝑃𝑘𝑘𝜕𝜕𝑞𝑞𝑖𝑖

�̇�𝑞𝑘𝑘�̇�𝑞𝑃𝑃 =

=12�𝑃𝑃=1

𝑛𝑛

�𝑘𝑘=1


�̇�𝑞𝑘𝑘�̇�𝑞𝑃𝑃 +12�𝑃𝑃=1

𝑛𝑛

�𝑘𝑘=1

𝑛𝑛𝜕𝜕𝑏𝑏𝑖𝑖𝑘𝑘𝜕𝜕𝑞𝑞𝑃𝑃

−𝜕𝜕𝑏𝑏𝑃𝑃𝑘𝑘𝜕𝜕𝑞𝑞𝑖𝑖

�̇�𝑞𝑘𝑘�̇�𝑞𝑃𝑃

The generic element of 𝐂𝐂 is:

𝑐𝑐𝑖𝑖𝑃𝑃 = �𝑘𝑘=1

𝑛𝑛

𝑐𝑐𝑖𝑖𝑃𝑃𝑘𝑘�̇�𝑞𝑘𝑘

𝑐𝑐𝑖𝑖𝑃𝑃𝑘𝑘 =12

𝜕𝜕𝑏𝑏𝑖𝑖𝑃𝑃𝜕𝜕𝑞𝑞𝑘𝑘

+𝜕𝜕𝑏𝑏𝑖𝑖𝑘𝑘𝜕𝜕𝑞𝑞𝑃𝑃


Christoffel symbols of the first kindwhere:

Computation of the elements of C


The previous choice of matrix 𝐂𝐂 allows to prove an important property of the dynamic model of the manipulator. Matrix:

𝑐𝑐𝑖𝑖𝑃𝑃 =12 �𝑘𝑘=1


�̇�𝑞𝑘𝑘 +12 �𝑘𝑘=1



�̇�𝑞𝑘𝑘 =12 �̇�𝑏𝑖𝑖𝑃𝑃

+12 �𝑘𝑘=1



�̇�𝑞𝑘𝑘

𝐍𝐍 𝐪𝐪, �̇�𝐪 = �̇�𝐁 𝐪𝐪 − 2𝐂𝐂 𝐪𝐪, �̇�𝐪

is skew-symmetric:

𝐰𝐰𝑇𝑇𝐍𝐍 𝐪𝐪, �̇�𝐪 𝐰𝐰 = 0, ∀𝐰𝐰

In fact:

𝑛𝑛𝑖𝑖𝑃𝑃 = �̇�𝑏𝑖𝑖𝑃𝑃 − 2𝑐𝑐𝑖𝑖𝑃𝑃 = �𝑘𝑘=1

𝑛𝑛𝜕𝜕𝑏𝑏𝑃𝑃𝑘𝑘𝜕𝜕𝑞𝑞𝑖𝑖

−𝜕𝜕𝑏𝑏𝑖𝑖𝑘𝑘𝜕𝜕𝑞𝑞𝑃𝑃

�̇�𝑞𝑘𝑘 𝑛𝑛𝑖𝑖𝑃𝑃 = −𝑛𝑛𝑃𝑃𝑖𝑖 (skew-symmetric)

Skew-symmetry of matrix �̇�𝐁 − 2𝐂𝐂


The equation:

�̇�𝐪𝑇𝑇𝐍𝐍 𝐪𝐪, �̇�𝐪 �̇�𝐪 = 0

(particular case of the previous one) is valid whatever the choice of matrix 𝐂𝐂 is. From the energy conservation principle, the derivative of the kinetic energy equals the power generated by all the forces acting at the joint of the manipulator:

12𝜌𝜌𝜌𝜌𝑑𝑑 �̇�𝐪𝑇𝑇𝐁𝐁 𝐪𝐪 �̇�𝐪 = �̇�𝐪𝑇𝑇 τ − 𝐅𝐅𝑣𝑣�̇�𝐪 − 𝐟𝐟𝑠𝑠 𝐪𝐪, �̇�𝐪 − 𝐠𝐠 𝐪𝐪

Taking the derivative at the left hand side and using the equation of the model:

12𝜌𝜌𝜌𝜌𝑑𝑑 �̇�𝐪𝑇𝑇𝐁𝐁 𝐪𝐪 �̇�𝐪 =

12 �̇�𝐪

𝑇𝑇�̇�𝐁 𝐪𝐪 �̇�𝐪 + �̇�𝐪𝑇𝑇𝐁𝐁 𝐪𝐪 �̈�𝐪 =12 �̇�𝐪

𝑇𝑇 �̇�𝐁 𝐪𝐪 − 2𝐂𝐂 𝐪𝐪, �̇�𝐪 �̇�𝐪 + �̇�𝐪𝑇𝑇 τ − 𝐅𝐅𝑣𝑣�̇�𝐪 − 𝐟𝐟𝑠𝑠 𝐪𝐪, �̇�𝐪 − 𝐠𝐠 𝐪𝐪

from which the equation follows.

Energy conservation


If we assume a simplified expression for the static friction function:

𝐟𝐟𝑠𝑠 𝐪𝐪, �̇�𝐪 = 𝐅𝐅𝑠𝑠 sgn �̇�𝐪

π: vector of 𝑝𝑝 constant parameters

𝐘𝐘 : 𝑛𝑛 × 𝑝𝑝 matrix, function of joint positions, velocities and accelerations (regression matrix)

τ = 𝐘𝐘 𝐪𝐪, �̇�𝐪, �̈�𝐪 π

it is possible to prove that the dynamic model of the manipulator is linear with respect to a suitable set of dynamic parameters (masses, moments of inertia)We can then write:

Linearity in the dynamical parameters


Consider a two-link Cartesian manipulator, characterized by link masses 𝑚𝑚1 and 𝑚𝑚2.The vector of generalized coordinates is:

The Jacobians needed for the computation of the inertia matrix are:

𝐪𝐪 = 𝜌𝜌1𝜌𝜌2

𝐉𝐉𝑃𝑃𝑙𝑙1 = 𝐣𝐣𝑃𝑃1

𝑙𝑙1 0 = 𝐳𝐳0 0 =0 00 01 0


𝑙𝑙2 𝐣𝐣𝑃𝑃2𝑙𝑙2 = 𝐳𝐳0 𝐳𝐳1 =

0 10 01 0

while there are no contributions to the angular velocities.

x0

z0

z1

m1

m2

d1

d2

Two-link Cartesian manipulator


Computing the inertia matrix with the general formula, we obtain:

As 𝐁𝐁 is constant, 𝐂𝐂 = 0, i.e. there are no centrifugal and Coriolis terms.Then, since:

the vector of the gravitational terms is:

𝐠𝐠0 =00−𝑚𝑚

𝐁𝐁 = 𝑚𝑚1𝐉𝐉𝑃𝑃𝑙𝑙1 𝑇𝑇𝐉𝐉𝑃𝑃

𝑙𝑙1 + 𝑚𝑚2𝐉𝐉𝑃𝑃𝑙𝑙2 𝑇𝑇𝐉𝐉𝑃𝑃

𝑙𝑙2 = 𝑚𝑚1 + 𝑚𝑚2 00 𝑚𝑚2

𝐠𝐠 = 𝑚𝑚1 + 𝑚𝑚2 𝑚𝑚0



If there are no friction torques and no forces at the end-effector:

𝑚𝑚1 + 𝑚𝑚2 �̈�𝜌1 + 𝑚𝑚1 + 𝑚𝑚2 𝑚𝑚 = 𝑓𝑓1𝑚𝑚2�̈�𝜌2 = 𝑓𝑓2

𝑓𝑓1 e 𝑓𝑓2 : forces which act along the generalized coordinates



Let us consider a two-link planar manipulator: masses: 𝑚𝑚1 and 𝑚𝑚2 lengths: 𝑎𝑎1 and 𝑎𝑎2 distances of the centers of mass from the joint axes: 𝑙𝑙1 and 𝑙𝑙2 moments of inertia around axes passing through the centers of

mass and parallel to 𝐳𝐳0 : 𝐼𝐼1 and 𝐼𝐼2

𝐪𝐪 = 𝜗𝜗1𝜗𝜗2

m1, I1

m2, I2

a1

a2

ϑ1

ϑ2

l1

l2

x0

y0

The Jacobians needed for the computation of the inertia matrix depend on the following vectors:

𝐩𝐩𝑙𝑙1 =𝑚1𝑐𝑐1𝑚1𝑠𝑠1

0, 𝐩𝐩𝑙𝑙2 =

𝑎𝑎1𝑐𝑐1 + 𝑚2𝑐𝑐12𝑎𝑎1𝑠𝑠1 + 𝑚2𝑠𝑠12

0, 𝐩𝐩0 =

000

, 𝐩𝐩1 =𝑎𝑎1𝑐𝑐1𝑎𝑎1𝑠𝑠1

0

Generalized coordinates:

𝐳𝐳0 = 𝐳𝐳1 =001

Two-link planar manipulator


Link 1


𝑙𝑙1 0 = 𝐳𝐳0 × 𝐩𝐩𝑙𝑙1 − 𝐩𝐩0 0 =−𝑚1𝑠𝑠1 0𝑚1𝑐𝑐1 0

0 0

𝐉𝐉𝑂𝑂𝑙𝑙1 = 𝐣𝐣𝑂𝑂1

𝑙𝑙1 0 = 𝐳𝐳0 0 =0 00 01 0

Link 2


𝑙𝑙2 𝐣𝐣𝑃𝑃2𝑙𝑙2 = 𝐳𝐳0 × 𝐩𝐩𝑙𝑙2 − 𝐩𝐩0 𝐳𝐳1 × 𝐩𝐩𝑙𝑙2 − 𝐩𝐩1 =

−𝑎𝑎1𝑠𝑠1 − 𝑚2𝑠𝑠12 −𝑚2𝑠𝑠12𝑎𝑎1𝑐𝑐1 + 𝑚2𝑐𝑐12 𝑚2𝑐𝑐12

0 0

𝐉𝐉𝑂𝑂𝑙𝑙2 = 𝐣𝐣𝑂𝑂1

𝑙𝑙2 𝐣𝐣𝑂𝑂2𝑙𝑙2 = 𝐳𝐳0 𝐳𝐳1 =

0 00 01 1



Taking into account that the angular velocity vectors 𝛚𝛚1 and 𝛚𝛚2 are aligned with 𝐳𝐳0 it is not necessary to compute rotation matrices 𝐑𝐑𝑖𝑖 , so that the computation of the inertia matrix gives:

𝐁𝐁 𝐪𝐪 = 𝑚𝑚1𝐉𝐉𝑃𝑃𝑙𝑙1 𝑇𝑇𝐉𝐉𝑃𝑃

𝑙𝑙1 + 𝑚𝑚2𝐉𝐉𝑃𝑃𝑙𝑙2 𝑇𝑇𝐉𝐉𝑃𝑃

𝑙𝑙2 + 𝐼𝐼1𝐉𝐉𝑂𝑂𝑙𝑙1 𝑇𝑇𝐉𝐉𝑂𝑂

𝑙𝑙1 + 𝐼𝐼2𝐉𝐉𝑂𝑂𝑙𝑙2 𝑇𝑇𝐉𝐉𝑂𝑂

𝑙𝑙2 = 𝑏𝑏11 𝜗𝜗 𝑏𝑏12 𝜗𝜗𝑏𝑏21 𝜗𝜗 𝑏𝑏22

depends on ϑ2depends on ϑ2constant!


𝑏𝑏11 = 𝑚𝑚1𝑚12 + 𝐼𝐼1 + 𝑚𝑚2 𝑎𝑎12 + 𝑚22 + 2𝑎𝑎1𝑚2𝑐𝑐2 + 𝐼𝐼2𝑏𝑏12 = 𝑏𝑏21 = 𝑚𝑚2 𝑚22 + 𝑎𝑎1𝑚2𝑐𝑐2 + 𝐼𝐼2𝑏𝑏22 = 𝑚𝑚2𝑚22 + 𝐼𝐼2


From the inertia matrix it is simple to compute the Christoffel symbols:

𝑐𝑐111 =12𝜕𝜕𝑏𝑏11𝜕𝜕𝑞𝑞1

= 0

𝑐𝑐112 = 𝑐𝑐121 =12𝜕𝜕𝑏𝑏11𝜕𝜕𝑞𝑞2

= −𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2 = ℎ

𝑐𝑐122 =𝜕𝜕𝑏𝑏12𝜕𝜕𝑞𝑞2

−12𝜕𝜕𝑏𝑏22𝜕𝜕𝑞𝑞1

= −𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2 = ℎ

𝑐𝑐211 =𝜕𝜕𝑏𝑏21𝜕𝜕𝑞𝑞1

−12𝜕𝜕𝑏𝑏11𝜕𝜕𝑞𝑞2

= 𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2 = −ℎ

𝑐𝑐212 = 𝑐𝑐221 =12𝜕𝜕𝑏𝑏22𝜕𝜕𝑞𝑞1

= 0

𝑐𝑐222 =12𝜕𝜕𝑏𝑏22𝜕𝜕𝑞𝑞2

= 0

ℎ = −𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2



The expression of matrix 𝐂𝐂 is then:

𝐂𝐂 𝐪𝐪, �̇�𝐪 =ℎ�̇�𝜗2 ℎ �̇�𝜗1 + �̇�𝜗2−ℎ�̇�𝜗1 0

=−𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗2 −𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2 �̇�𝜗1 + �̇�𝜗2𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗1 0

We can verify that matrix 𝐍𝐍 is skew-symmetric:

𝐍𝐍 𝐪𝐪, �̇�𝐪 = �̇�𝐁 𝐪𝐪 − 2𝐂𝐂 𝐪𝐪, �̇�𝐪

= 2ℎ�̇�𝜗2 ℎ�̇�𝜗2ℎ�̇�𝜗2 0

− 2ℎ�̇�𝜗2 ℎ �̇�𝜗1 + �̇�𝜗2−ℎ�̇�𝜗1 0

=

= 0 −2ℎ�̇�𝜗1 − ℎ�̇�𝜗22ℎ�̇�𝜗1 + ℎ�̇�𝜗2 0

= −𝐍𝐍𝑇𝑇 𝐪𝐪, �̇�𝐪

Furthermore, since 𝐠𝐠0 =0−𝑚𝑚0

, the

vector of the gravitational terms is:𝐠𝐠 = 𝑚𝑚1𝑚1 + 𝑚𝑚2𝑎𝑎1 𝑚𝑚𝑐𝑐1 + 𝑚𝑚2𝑚𝑚𝑚2𝑐𝑐12

𝑚𝑚2𝑚𝑚𝑚2𝑐𝑐12



𝜏𝜏𝑖𝑖 : torques applied at the joints

𝑚𝑚1𝑚12 + 𝐼𝐼1 + 𝑚𝑚2 𝑎𝑎12 + 𝑚22 + 2𝑎𝑎1𝑚2𝑐𝑐2 + 𝐼𝐼2 �̈�𝜗1 + 𝑚𝑚2 𝑚22 + 𝑎𝑎1𝑚2𝑐𝑐2 + 𝐼𝐼2 �̈�𝜗2 +−2𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗1�̇�𝜗2 − 𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗22 ++ 𝑚𝑚1𝑚1 + 𝑚𝑚2𝑎𝑎1 𝑚𝑚𝑐𝑐1 + 𝑚𝑚2𝑚𝑚𝑚2𝑐𝑐12 = 𝜏𝜏1

𝑚𝑚2 𝑚22 + 𝑎𝑎1𝑚2𝑐𝑐2 + 𝐼𝐼2 �̈�𝜗1 + 𝑚𝑚2𝑚22 + 𝐼𝐼2 �̈�𝜗2 ++𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗1

2 + 𝑚𝑚2𝑚2𝑚𝑚𝑐𝑐12 = 𝜏𝜏2

Without friction at the joints, the equations of motion are:



By simple inspection, we can obtain the dynamical parameters with respect to which the model is linear, i.e. those parameters for which we can write:

τ = 𝐘𝐘 𝐪𝐪, �̇�𝐪, �̈�𝐪 π

We have:

π = 𝜋𝜋1 𝜋𝜋2 𝜋𝜋3 𝜋𝜋4 𝜋𝜋5 𝑇𝑇

𝜋𝜋1 = 𝑚𝑚1𝑚1𝜋𝜋2 = 𝐼𝐼1 + 𝑚𝑚1𝑚12𝜋𝜋3 = 𝑚𝑚2𝜋𝜋4 = 𝑚𝑚2𝑚2𝜋𝜋5 = 𝐼𝐼2 + 𝑚𝑚2𝑚22



𝑦𝑦11 = 𝑚𝑚𝑐𝑐1𝑦𝑦12 = �̈�𝜗1𝑦𝑦13 = 𝑎𝑎12�̈�𝜗1 + 𝑎𝑎1𝑚𝑚𝑐𝑐1𝑦𝑦14 = 2𝑎𝑎1𝑐𝑐2�̈�𝜗1 + 𝑎𝑎1𝑐𝑐2�̈�𝜗2 − 2𝑎𝑎1𝑠𝑠2�̇�𝜗1�̇�𝜗2 − 𝑎𝑎1𝑠𝑠2�̇�𝜗22 + 𝑚𝑚𝑐𝑐12𝑦𝑦15 = �̈�𝜗1 + �̈�𝜗2𝑦𝑦24 = 𝑎𝑎1𝑐𝑐2�̈�𝜗1 + 𝑎𝑎1𝑠𝑠2�̇�𝜗12 + 𝑚𝑚𝑐𝑐12𝑦𝑦25 = �̈�𝜗1 + �̈�𝜗2

𝐘𝐘 𝐪𝐪, �̇�𝐪, �̈�𝐪 =𝑦𝑦11 𝑦𝑦12 𝑦𝑦13 𝑦𝑦14 𝑦𝑦150 0 0 𝑦𝑦24 𝑦𝑦25

The coefficients of 𝐘𝐘 depend on 𝜗𝜗1, 𝜗𝜗2, their first and second derivatives, 𝑚𝑚 and 𝑎𝑎1.



The linearity of the dynamic model with respect to the dynamical parameters:

τ = 𝐘𝐘 𝐪𝐪, �̇�𝐪, �̈�𝐪 π

allows to setup a procedure for the experimental identification of the same parameters, which are usually unknown or uncertain.

Suitable motion trajectories must be executed, along which the joint positions 𝐪𝐪 are recorded, the velocities �̇�𝐪 are measured or obtained by numerical differentiation, and the accelerations �̈�𝐪 are obtained with filtered (also non-causal) differentiation.Also the torques τ are measured, directly (with suitable sensors) or indirectly, from the measurements of currents in the motors.

Suppose to have the measurements (direct or indirect ones) of all the variables for the time instants 𝑑𝑑1, 𝑑𝑑2,⋯ , 𝑑𝑑𝑁𝑁

Identification of dynamical parameters


With N measurement sets:

τ̄ =τ̄ 𝑑𝑑1⋮

τ̄ 𝑑𝑑𝑁𝑁=

𝐘𝐘 𝑑𝑑1⋮

𝐘𝐘 𝑑𝑑𝑁𝑁π = �̄�𝐘π

Solving with a least-squares technique:

π = �̄�𝐘𝑇𝑇�̄�𝐘 −1�̄�𝐘𝑇𝑇τ̄ Left pseudo-inverse matrix of �̄�𝐘

Only the elements of π for which the corresponding column is different from zero can be identified.

Some parameters are identifiable only in combination with other ones. The trajectories to be used must be sufficiently rich (good conditioning of matrix �̄�𝐘𝑇𝑇�̄�𝐘): they

need to involve all components in the dynamic model

Identification of dynamical parameters


An alternative way to formulate the dynamic model of the manipulator is the Newton-Euler method.It is based on balances of forces and moments acting on the single link, due to the interactions with the nearby links in the kinematic chain.

We obtain a system of equations that might be solved in a recursive way, propagating the velocities and accelerations from the base to the end effector, while the forces and moments in the opposite way:

Recursion makes Newton-Euler algorithm computationally efficient.

velo

cities

acce

lerati

ons

forc

esm

omen

ts

Newton-Euler formulation


Let us consider the generic link i of the kinematic chain:

𝑚𝑚𝑖𝑖 and 𝐈𝐈𝑖𝑖 mass and inertia tensor of the link𝐫𝐫𝑖𝑖−1,𝐶𝐶𝑖𝑖 vector from the origin of frame (𝑖𝑖 − 1) to the center of mass 𝐶𝐶𝑖𝑖𝐫𝐫𝑖𝑖,𝐶𝐶𝑖𝑖 vector from the origin of frame 𝑖𝑖 to the center of mass 𝐶𝐶𝑖𝑖𝐫𝐫𝑖𝑖−1,𝑖𝑖 vector from the origin of frame (𝑖𝑖 − 1) to the origin of frame 𝑖𝑖

We define the following parameters:

This picture is taken from the textbook:

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo: Robotics: Modelling, Planning and Control, 3rd Ed. Springer, 2009

Definition of the parameters


�̇�𝐩𝐶𝐶𝑖𝑖 linear velocity of the center of mass 𝐶𝐶𝑖𝑖�̇�𝐩𝑖𝑖 linear velocity of the origin of frame 𝑖𝑖ω𝑖𝑖 angular velocity of the link�̈�𝐩𝐶𝐶𝑖𝑖 linear acceleration of the center of mass 𝐶𝐶𝑖𝑖�̈�𝐩𝑖𝑖 linear acceleration of the origin of frame 𝑖𝑖ω̇𝑖𝑖 angular acceleration of the link𝐠𝐠0 gravity acceleration𝐟𝐟𝑖𝑖 force exerted by link 𝑖𝑖 − 1 on link 𝑖𝑖−𝐟𝐟𝑖𝑖+1 force exerted by link 𝑖𝑖 + 1 on link 𝑖𝑖μ𝑖𝑖 moment exerted by link 𝑖𝑖 − 1 on link 𝑖𝑖 with respect to the origin of frame 𝑖𝑖 − 1−μ𝑖𝑖+1 moment exerted by link 𝑖𝑖 + 1 on link 𝑖𝑖 with respect to the origin of frame 𝑖𝑖All vectors are expressed in the base frame.

Definition of the variables


Newton’s equation (translational motion of the center of mass)

𝐟𝐟𝑖𝑖 − 𝐟𝐟𝑖𝑖+1 + 𝑚𝑚𝑖𝑖𝐠𝐠0 = 𝑚𝑚𝑖𝑖�̈�𝐩𝐶𝐶𝑖𝑖

Euler’s equation (rotational motion)

μ𝑖𝑖 + 𝐟𝐟𝑖𝑖 × 𝐫𝐫𝑖𝑖−1,𝐶𝐶𝑖𝑖 − μ𝑖𝑖+1 − 𝐟𝐟𝑖𝑖+1 × 𝐫𝐫𝑖𝑖,𝐶𝐶𝑖𝑖 =𝜌𝜌𝜌𝜌𝑑𝑑

𝐈𝐈𝑖𝑖ω𝑖𝑖 = 𝐈𝐈𝑖𝑖ω̇𝑖𝑖 + ω𝑖𝑖 × 𝐈𝐈𝑖𝑖ω𝑖𝑖

Generalized force at joint 𝑖𝑖:

it can be proven gyroscopic effect

𝜏𝜏𝑖𝑖 = �𝐟𝐟𝑖𝑖𝑇𝑇𝐳𝐳𝑖𝑖−1 prismatic jointμ𝑖𝑖𝑇𝑇𝐳𝐳𝑖𝑖−1 rotationa𝑚 joint



Propagation of the velocities:

ω𝑖𝑖 = �ω𝑖𝑖−1 prismatic joint

ω𝑖𝑖−1 + �̇�𝜗𝑖𝑖𝐳𝐳𝑖𝑖−1 rotationa𝑚 joint

�̇�𝐩𝑖𝑖 = ��̇�𝐩𝑖𝑖−1 + �̇�𝜌𝑖𝑖𝐳𝐳𝑖𝑖−1 + ω𝑖𝑖 × 𝐫𝐫𝑖𝑖−1,𝑖𝑖 prismatic joint�̇�𝐩𝑖𝑖−1 + ω𝑖𝑖 × 𝐫𝐫𝑖𝑖−1,𝑖𝑖 rotationa𝑚 joint

Propagation of the accelerations:

ω̇𝑖𝑖 = �ω̇𝑖𝑖−1 prismatic joint

ω̇𝑖𝑖−1 + �̈�𝜗𝑖𝑖𝐳𝐳𝑖𝑖−1 + �̇�𝜗𝑖𝑖ω𝑖𝑖−1 × 𝐳𝐳𝑖𝑖−1 rotationa𝑚 joint

�̈�𝐩𝑖𝑖 = ��̈�𝐩𝑖𝑖−1 + �̈�𝜌𝑖𝑖𝐳𝐳𝑖𝑖−1 + 2�̇�𝜌𝑖𝑖ω𝑖𝑖 × 𝐳𝐳𝑖𝑖−1 + ω̇𝑖𝑖 × 𝐫𝐫𝑖𝑖−1,𝑖𝑖 + ω𝑖𝑖 × ω𝑖𝑖 × 𝐫𝐫𝑖𝑖−1,𝑖𝑖 prismatic joint

�̈�𝐩𝑖𝑖−1 + ω̇𝑖𝑖 × 𝐫𝐫𝑖𝑖−1,𝑖𝑖 + ω𝑖𝑖 × ω𝑖𝑖 × 𝐫𝐫𝑖𝑖−1,𝑖𝑖 rotationa𝑚 joint

�̈�𝐩𝐶𝐶𝑖𝑖 = �̈�𝐩𝑖𝑖 + ω̇𝑖𝑖 × 𝐫𝐫𝑖𝑖,𝐶𝐶𝑖𝑖 + ω𝑖𝑖 × ω𝑖𝑖 × 𝐫𝐫𝑖𝑖,𝐶𝐶𝑖𝑖 center of mass

Note: derivative of a vector 𝐚𝐚𝑖𝑖attached to the moving frame 𝑖𝑖

𝜌𝜌𝜌𝜌𝑑𝑑𝐚𝐚𝑖𝑖 𝑑𝑑 =

𝜌𝜌𝜌𝜌𝑑𝑑𝐑𝐑𝑖𝑖 𝑑𝑑 𝐚𝐚𝑖𝑖𝑖𝑖 =

= 𝑆𝑆 ω𝑖𝑖 𝑑𝑑 𝐑𝐑𝑖𝑖 𝑑𝑑 𝐚𝐚𝑖𝑖𝑖𝑖 = ω𝑖𝑖 𝑑𝑑 × 𝐚𝐚𝑖𝑖 𝑑𝑑

Accelerations of a link


A forward recursion of velocities and accelerations is made: initial conditions on ω0, �̈�𝐩0 − 𝐠𝐠0, ω̇0

computation of ω𝑖𝑖 , ω̇𝑖𝑖 , �̈�𝐩𝑖𝑖 , �̈�𝐩𝐶𝐶𝑖𝑖A backward recursion of forces and moments is made:

terminal conditions on 𝐟𝐟𝑛𝑛+1 and μ𝑛𝑛+1 computations:

𝐟𝐟𝑖𝑖 = 𝐟𝐟𝑖𝑖+1 + 𝑚𝑚𝑖𝑖�̈�𝐩𝐶𝐶𝑖𝑖μ𝑖𝑖 = −𝐟𝐟𝑖𝑖 × 𝐫𝐫𝑖𝑖−1,𝑖𝑖 + 𝐫𝐫𝑖𝑖,𝐶𝐶𝑖𝑖 + μ𝑖𝑖+1 + 𝐟𝐟𝑖𝑖+1 × 𝐫𝐫𝑖𝑖,𝐶𝐶𝑖𝑖 + 𝐈𝐈𝑖𝑖ω̇𝑖𝑖 + ω𝑖𝑖 × 𝐈𝐈𝑖𝑖ω𝑖𝑖

The generalized force at joint 𝑖𝑖 (including friction contributions) is computed:

𝜏𝜏𝑖𝑖 = �𝐟𝐟𝑖𝑖𝑇𝑇𝐳𝐳𝑖𝑖−1 + 𝐹𝐹𝑣𝑣𝑖𝑖�̇�𝜌𝑖𝑖 + 𝑓𝑓𝑠𝑠𝑖𝑖 prismatic jointμ𝑖𝑖𝑇𝑇𝐳𝐳𝑖𝑖−1 + 𝐹𝐹𝑣𝑣𝑖𝑖�̇�𝜗𝑖𝑖 + 𝑓𝑓𝑠𝑠𝑖𝑖 rotationa𝑚 joint

velo

cities

acce

lerati

ons

forc

esm

omen

ts

Recursive algorithm


Up to now we have supposed that all the vectors are referred to the base frame.

It is more convenient to express the vectors with respect to the current frame on link i. In this way, vectors 𝐫𝐫𝑖𝑖−1,𝑖𝑖 and 𝐫𝐫𝑖𝑖,𝐶𝐶𝑖𝑖 and the inertia tensor 𝐈𝐈𝑖𝑖 are constant, which makes the algorithm computationally more efficient.

The equations are modified in some terms (we need to multiply the vectors by suitable rotation matrices) but nothing changes in the nature of the method.

Local reference frames


Let us consider again a two-link planar manipulator with rotational joints, whose model has been already derived with the Euler-Lagrange method: masses: 𝑚𝑚1 and 𝑚𝑚2 lengths: 𝑎𝑎1 and 𝑎𝑎2 distances of the centers of mass from the joint axes: 𝑙𝑙1 and 𝑙𝑙2 moments of inertia around axes passing through the centers of

mass and parallel to 𝐳𝐳0 : 𝐼𝐼1 and 𝐼𝐼2

Initial conditions for the forward recursion of velocities and accelerations;

�̈�𝐩00 − 𝐠𝐠00 = 0 𝑚𝑚 0 𝑇𝑇 , ω00 = ω̇0

0 = 0

m1, I1

m2, I2

a1

a2

ϑ1

ϑ2

l1

l2

x0

y0

Initial conditions for the backward recursion of forces and moments:

𝐟𝐟33 = 0, μ33 = 0



Let us refer all the quantities to the current frame on the link. We derive these constant vectors:

The rotation matrices are:

𝐫𝐫1,𝐶𝐶11 =

𝑚1 − 𝑎𝑎100

, 𝐫𝐫0,11 =

𝑎𝑎100

, 𝐫𝐫2,𝐶𝐶22 =

𝑚2 − 𝑎𝑎200

, 𝐫𝐫1,22 =

𝑎𝑎200

𝐑𝐑10 =𝑐𝑐1 −𝑠𝑠1 0𝑠𝑠1 𝑐𝑐1 00 0 1

, 𝐑𝐑21 =𝑐𝑐2 −𝑠𝑠2 0𝑠𝑠2 𝑐𝑐2 00 0 1

, 𝐑𝐑32 =1 0 00 1 00 0 1

Definition of vectors and matrices


ω11 = 𝐑𝐑10𝑇𝑇 ω0 + �̇�𝜗1𝐳𝐳0 =

00�̇�𝜗1

ω̇11 = 𝐑𝐑10𝑇𝑇 ω̇0 + �̈�𝜗1𝐳𝐳0 + �̇�𝜗1ω0 × 𝐳𝐳0 =

00�̈�𝜗1

�̈�𝐩11 = 𝐑𝐑10𝑇𝑇�̈�𝐩0 + ω̇11 × 𝐫𝐫0,1

1 + ω11 × ω1

1 × 𝐫𝐫0,11 =

−𝑎𝑎1�̇�𝜗12 + 𝑚𝑚𝑠𝑠1𝑎𝑎1�̈�𝜗1 + 𝑚𝑚𝑐𝑐1

0

�̈�𝐩𝐶𝐶11 = �̈�𝐩11 + ω̇1

1 × 𝐫𝐫1,𝐶𝐶11 + ω1

1 × ω11 × 𝐫𝐫1,𝐶𝐶1

1 =−𝑚1�̇�𝜗12 + 𝑚𝑚𝑠𝑠1𝑚1�̈�𝜗1 + 𝑚𝑚𝑐𝑐1

0

Forward recursion: link 1


ω22 = 𝐑𝐑21𝑇𝑇 ω1

1 + �̇�𝜗2𝐳𝐳0 =00

�̇�𝜗1 + �̇�𝜗2

ω̇22 = 𝐑𝐑21𝑇𝑇 ω̇1

1 + �̈�𝜗2𝐳𝐳0 + �̇�𝜗2ω11 × 𝐳𝐳0 =00

�̈�𝜗1 + �̈�𝜗2

�̈�𝐩22 = 𝐑𝐑21𝑇𝑇�̈�𝐩11 + ω̇22 × 𝐫𝐫1,2

2 + ω22 × ω2

2 × 𝐫𝐫1,22 =

𝑎𝑎1𝑠𝑠2�̈�𝜗1 − 𝑎𝑎1𝑐𝑐2�̇�𝜗12 − 𝑎𝑎2 �̇�𝜗1 + �̇�𝜗22 + 𝑚𝑚𝑠𝑠12

𝑎𝑎1𝑐𝑐2�̈�𝜗1 + 𝑎𝑎2 �̈�𝜗1 + �̈�𝜗2 + 𝑎𝑎1𝑠𝑠2�̇�𝜗12 + 𝑚𝑚𝑐𝑐120

�̈�𝐩𝐶𝐶22 = �̈�𝐩22 + ω̇22 × 𝐫𝐫2,𝐶𝐶2

2 + ω22 × ω2

2 × 𝐫𝐫2,𝐶𝐶22 =

𝑎𝑎1𝑠𝑠2�̈�𝜗1 − 𝑎𝑎1𝑐𝑐2�̇�𝜗12 − 𝑚2 �̇�𝜗1 + �̇�𝜗22 + 𝑚𝑚𝑠𝑠12

𝑎𝑎1𝑐𝑐2�̈�𝜗1 + 𝑚2 �̈�𝜗1 + �̈�𝜗2 + 𝑎𝑎1𝑠𝑠2�̇�𝜗12 + 𝑚𝑚𝑐𝑐120

Forward recursion: link 2


𝐟𝐟22 = 𝐑𝐑32𝐟𝐟33 + 𝑚𝑚2�̈�𝐩𝐶𝐶22 = 𝑚𝑚2�̈�𝐩𝐶𝐶2

2

=𝑚𝑚2 𝑎𝑎1𝑠𝑠2�̈�𝜗1 − 𝑎𝑎1𝑐𝑐2�̇�𝜗12 − 𝑚2 �̇�𝜗1 + �̇�𝜗2

2 + 𝑚𝑚𝑠𝑠12𝑚𝑚2 𝑎𝑎1𝑐𝑐2�̈�𝜗1 + 𝑚2 �̈�𝜗1 + �̈�𝜗2 + 𝑎𝑎1𝑠𝑠2�̇�𝜗12 + 𝑚𝑚𝑐𝑐12

0

μ22 = −𝐟𝐟22 × 𝐫𝐫1,22 + 𝐫𝐫2,𝐶𝐶2

2 + 𝐑𝐑32μ33 + 𝐑𝐑32𝐟𝐟33 × 𝐫𝐫2,𝐶𝐶22 + 𝐈𝐈22ω̇2

2 + ω22 × 𝐈𝐈22ω2

2 =

=∗∗

𝐼𝐼2 + 𝑚𝑚2𝑚22 �̈�𝜗1 + �̈�𝜗2 + 𝑚𝑚2𝑎𝑎1𝑚2𝑐𝑐2�̈�𝜗1 + 𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗12 + 𝑚𝑚2𝑚2𝑚𝑚𝑐𝑐12

𝜏𝜏2 = μ22𝑇𝑇𝐑𝐑21𝑇𝑇𝐳𝐳0 == 𝐼𝐼2 + 𝑚𝑚2 𝑚22 + 𝑎𝑎1𝑚2𝑐𝑐2 �̈�𝜗1 + 𝐼𝐼2 + 𝑚𝑚2𝑚22 �̈�𝜗2 + 𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗12

(identical to the equation obtained with Euler-Lagrange method)

Backward recursion: link 2


𝐟𝐟11 = 𝐑𝐑21𝐟𝐟22 + 𝑚𝑚2�̈�𝐩𝐶𝐶11

=−𝑚𝑚2𝑚2𝑠𝑠2 �̈�𝜗1 + �̈�𝜗2 − 𝑚𝑚1𝑚1�̇�𝜗12 − 𝑚𝑚2𝑎𝑎1�̇�𝜗12 − 𝑚𝑚2𝑚2𝑐𝑐2 �̇�𝜗1 + �̇�𝜗2

2 + 𝑚𝑚1 + 𝑚𝑚2 𝑚𝑚𝑠𝑠1𝑚𝑚1𝑚1 + 𝑚𝑚2𝑎𝑎1 �̈�𝜗1 + 𝑚𝑚2𝑚2𝑐𝑐2 �̈�𝜗1 + �̈�𝜗2 − 𝑚𝑚2𝑚2𝑠𝑠2 �̇�𝜗1 + �̇�𝜗2

2 + 𝑚𝑚1 + 𝑚𝑚2 𝑚𝑚𝑐𝑐10

μ11 = −𝐟𝐟11 × 𝐫𝐫0,11 + 𝐫𝐫1,𝐶𝐶1

1 + 𝐑𝐑21μ22 + 𝐑𝐑21𝐟𝐟22 × 𝐫𝐫1,𝐶𝐶11 + 𝐈𝐈11ω̇11 + ω1

1 × 𝐈𝐈11ω11 =

=

∗∗

𝐼𝐼1 + 𝑚𝑚2𝑎𝑎12 + 𝑚𝑚1𝑚12 + 𝑚𝑚2𝑎𝑎1𝑚2𝑐𝑐2 �̈�𝜗1 + 𝐼𝐼2 + 𝑚𝑚2𝑎𝑎1𝑚2𝑐𝑐2 + 𝑚𝑚2𝑚22 �̈�𝜗1 + �̈�𝜗2 +

+𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗12 − 𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2 �̇�𝜗1 + �̇�𝜗22 + 𝑚𝑚1𝑚1 + 𝑚𝑚2𝑎𝑎1 𝑚𝑚𝑐𝑐1 + 𝑚𝑚2𝑚2𝑚𝑚𝑐𝑐12

𝜏𝜏1 = μ11𝑇𝑇𝐑𝐑10𝑇𝑇𝐳𝐳0 == 𝐼𝐼1 + 𝑚𝑚1𝑚12 + 𝐼𝐼2 + 𝑚𝑚2 𝑎𝑎12 + 𝑚22 + 2𝑎𝑎1𝑚2𝑐𝑐2 �̈�𝜗1 + 𝐼𝐼2 + 𝑚𝑚2 𝑚22 + 𝑎𝑎1𝑚2𝑐𝑐2 �̈�𝜗2

−2𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗1�̇�𝜗2 − 𝑚𝑚2𝑎𝑎1𝑚2𝑠𝑠2�̇�𝜗22 + 𝑚𝑚1𝑚1 + 𝑚𝑚2𝑎𝑎1 𝑚𝑚𝑐𝑐1 + 𝑚𝑚2𝑚2𝑚𝑚𝑐𝑐12

(identical to the equation obtained with Euler-Lagrange method)

Backward recursion: link 1


Euler-Lagrange formulation

it is systematic and easy to understand it returns the equations of motion in an analytic and compact form, separating the inertia matrix,

the Coriolis and centrifugal terms, the gravitational terms. All these elements are useful for the design of a model based controller

it lends itself to the introduction into the model of more complex effects (like joint or link deformation)


it is a computationally efficient recursive method

Euler-Lagrange vs. Newton-Euler


Dynamic model

s(1)=0;s(2)=pi/2;s(3)=0;s(4)=pi/2;s(5)=0;s(6)=0;ds(1)=0;ds(2)=0;ds(3)=0;ds(4)=0;ds(5)=0;ds(6)=0;dds(1)=0;dds(2)=0;dds(3)=0;dds(4)=0;dds(5)=0;dds(6)=0;

cq(1) = cos(s(1));sq(1) = sin(s(1));cq(2) = cos(s(2));sq(2) = sin(s(2));cq(3) = cos(s(3));sq(3) = sin(s(3));cq(4) = cos(s(4));sq(4) = sin(s(4));cq(5) = cos(s(5));sq(5) = sin(s(5));cq(6) = cos(s(6));sq(6) = sin(s(6));wy(1) = -ds(1);dwy(1) = -dds(1);dvsecz(1, 1) = -0.15*wy(1);dvterx(1, 1) = dvsecz(1, 1)*wy(1);dvquaz(1, 1) = -0.15*dwy(1);dvsecz(2, 1) = -0.15*wy(1);dvterx(2, 1) = dvsecz(2, 1)*wy(1);dvquaz(2, 1) = -0.15*dwy(1);aprx(1) = -0.023*wy(1);aprz(1) = 0.077*wy(1);asecx(1) = -0.023*dwy(1);asecz(1) = 0.077*dwy(1);aterx(1) = aprz(1)*wy(1);aterz(1) = -(aprx(1)*wy(1));ax(1) = asecx(1) + aterx(1) + dvterx(1, 1);az(1) = asecz(1) + aterz(1) + dvquaz(1, 1);fex(1) = 48.9*ax(1);fez(1) = 48.9*az(1);wprx(2) = sq(2)*wy(1);wpry(2) = cq(2)*wy(1);dwprx(2) = ds(2)*wy(1);dwx(2) = cq(2)*dwprx(2) + dwy(1)*sq(2);dwy(2) = cq(2)*dwy(1) - dwprx(2)*sq(2);dvprx(2, 2) = cq(2)*dvterx(2, 1) - 9.8062*sq(2);dvpry(2, 2) = -9.8062*cq(2) - dvterx(2, 1)*sq(2);dvsecy(2, 2) = -0.61*ds(2);dvsecz(2, 2) = 0.61*wpry(2);dvterx(2, 2) = -(ds(2)*dvsecy(2, 2)) + dvsecz(2, 2)*wpry(2);dvtery(2, 2) = -(dvsecz(2, 2)*wprx(2));

dvterz(2, 2) = dvsecy(2, 2)*wprx(2);dvquay(2, 2) = -0.61*dds(2);dvquaz(2, 2) = 0.61*dwy(2);vdx(2, 2) = dvprx(2, 2) + dvterx(2, 2);vdy(2, 2) = dvpry(2, 2) + dvquay(2, 2) + dvtery(2, 2);vdz(2, 2) = dvquaz(2, 1) + dvquaz(2, 2) + dvterz(2, 2);dvprx(3, 2) = cq(2)*dvterx(2, 1) - 9.8062*sq(2);dvpry(3, 2) = -9.8062*cq(2) - dvterx(2, 1)*sq(2);dvsecy(3, 2) = -0.61*ds(2);dvsecz(3, 2) = 0.61*wpry(2);dvterx(3, 2) = -(ds(2)*dvsecy(3, 2)) + dvsecz(3, 2)*wpry(2);dvtery(3, 2) = -(dvsecz(3, 2)*wprx(2));dvterz(3, 2) = dvsecy(3, 2)*wprx(2);dvquay(3, 2) = -0.61*dds(2);dvquaz(3, 2) = 0.61*dwy(2);vdx(3, 2) = dvprx(3, 2) + dvterx(3, 2);vdy(3, 2) = dvpry(3, 2) + dvquay(3, 2) + dvtery(3, 2);vdz(3, 2) = dvquaz(2, 1) + dvquaz(3, 2) + dvterz(3, 2);aprx(2) = 0.146*wpry(2);apry(2) = 0.776*ds(2) - 0.146*wprx(2);aprz(2) = -0.776*wpry(2);asecx(2) = 0.146*dwy(2);asecy(2) = 0.776*dds(2) - 0.146*dwx(2);asecz(2) = -0.776*dwy(2);aterx(2) = -(apry(2)*ds(2)) + aprz(2)*wpry(2);atery(2) = aprx(2)*ds(2) - aprz(2)*wprx(2);aterz(2) = apry(2)*wprx(2) - aprx(2)*wpry(2);ax(2) = asecx(2) + aterx(2) + vdx(2, 2);ay(2) = asecy(2) + atery(2) + vdy(2, 2);az(2) = asecz(2) + aterz(2) + vdz(2, 2);fex(2) = 90.57*ax(2);fey(2) = 90.57*ay(2);fez(2) = 90.57*az(2);wprx(3) = cq(3)*wprx(2) + sq(3)*wpry(2);wprz(3) = sq(3)*wprx(2) - cq(3)*wpry(2);wy(3) = ds(2) + ds(3);dwprx(3) = ds(3)*wpry(2);dwpry(3) = -(ds(3)*wprx(2));dwsecx(3) = dwprx(3) + dwx(2);dwsecy(3) = dwpry(3) + dwy(2);dwsecz(3) = dds(2) + dds(3);dwx(3) = cq(3)*dwsecx(3) + dwsecy(3)*sq(3);dwz(3) = -(cq(3)*dwsecy(3)) + dwsecx(3)*sq(3);dvprx(3, 3) = cq(3)*vdx(3, 2) + sq(3)*vdy(3, 2);dvprz(3, 3) = sq(3)*vdx(3, 2) - cq(3)*vdy(3, 2);dvsecy(3, 3) = -0.11*wprz(3);dvsecz(3, 3) = 0.11*wy(3);dvterx(3, 3) = -(dvsecy(3, 3)*wprz(3)) + dvsecz(3, 3)*wy(3);dvtery(3, 3) = -(dvsecz(3, 3)*wprx(3));dvterz(3, 3) = dvsecy(3, 3)*wprx(3);dvquay(3, 3) = -0.11*dwz(3);dvquaz(3, 3) = 0.11*dwsecz(3);vdx(3, 3) = dvprx(3, 3) + dvterx(3, 3);vdy(3, 3) = dvquay(3, 3) + dvtery(3, 3) + vdz(3, 2);vdz(3, 3) = dvprz(3, 3) + dvquaz(3, 3) + dvterz(3, 3);dvprx(4, 3) = cq(3)*vdx(3, 2) + sq(3)*vdy(3, 2);dvprz(4, 3) = sq(3)*vdx(3, 2) - cq(3)*vdy(3, 2);dvsecy(4, 3) = -0.11*wprz(3);dvsecz(4, 3) = 0.11*wy(3);dvterx(4, 3) = -(dvsecy(4, 3)*wprz(3)) + dvsecz(4, 3)*wy(3);

dvtery(4, 3) = -(dvsecz(4, 3)*wprx(3));dvterz(4, 3) = dvsecy(4, 3)*wprx(3);dvquay(4, 3) = -0.11*dwz(3);dvquaz(4, 3) = 0.11*dwsecz(3);vdx(4, 3) = dvprx(4, 3) + dvterx(4, 3);vdy(4, 3) = dvquay(4, 3) + dvtery(4, 3) + vdz(3, 2);vdz(4, 3) = dvprz(4, 3) + dvquaz(4, 3) + dvterz(4, 3);aprx(3) = 0.018*wprz(3) - 0.029*wy(3);apry(3) = 0.029*wprx(3) + 0.052*wprz(3);aprz(3) = -0.018*wprx(3) - 0.05200000000000001*wy(3);asecx(3) = -0.029*dwsecz(3) + 0.018*dwz(3);asecy(3) = 0.029*dwx(3) + 0.052*dwz(3);asecz(3) = -0.05200000000000001*dwsecz(3) - 0.018*dwx(3);aterx(3) = -(apry(3)*wprz(3)) + aprz(3)*wy(3);atery(3) = -(aprz(3)*wprx(3)) + aprx(3)*wprz(3);aterz(3) = apry(3)*wprx(3) - aprx(3)*wy(3);ax(3) = asecx(3) + aterx(3) + vdx(3, 3);ay(3) = asecy(3) + atery(3) + vdy(3, 3);az(3) = asecz(3) + aterz(3) + vdz(3, 3);fex(3) = 33*ax(3);fey(3) = 33*ay(3);fez(3) = 33*az(3);wprx(4) = cq(4)*wprx(3) + sq(4)*wy(3);wpry(4) = -wprz(3);wprz(4) = -(sq(4)*wprx(3)) + cq(4)*wy(3);wy(4) = -ds(4) + wpry(4);dwprx(4) = ds(4)*wy(3);dwpry(4) = -(ds(4)*wprx(3));dwsecx(4) = dwprx(4) + dwx(3);dwsecy(4) = dwpry(4) + dwsecz(3);dwsecz(4) = dds(4) + dwz(3);dwx(4) = cq(4)*dwsecx(4) + dwsecy(4)*sq(4);dwy(4) = -dwsecz(4);dwz(4) = cq(4)*dwsecy(4) - dwsecx(4)*sq(4);dvprx(4, 4) = cq(4)*vdx(4, 3) + sq(4)*vdy(4, 3);dvpry(4, 4) = -vdz(4, 3);dvprz(4, 4) = -(sq(4)*vdx(4, 3)) + cq(4)*vdy(4, 3);dvsecx(4, 4) = 0.61*wprz(4);dvsecz(4, 4) = -0.61*wprx(4);dvterx(4, 4) = dvsecz(4, 4)*wy(4);dvtery(4, 4) = -(dvsecz(4, 4)*wprx(4)) + dvsecx(4, 4)*wprz(4);dvterz(4, 4) = -(dvsecx(4, 4)*wy(4));dvquax(4, 4) = 0.61*dwz(4);dvquaz(4, 4) = -0.61*dwx(4);vdx(4, 4) = dvprx(4, 4) + dvquax(4, 4) + dvterx(4, 4);vdy(4, 4) = dvpry(4, 4) + dvtery(4, 4);vdz(4, 4) = dvprz(4, 4) + dvquaz(4, 4) + dvterz(4, 4);dvprx(5, 4) = cq(4)*vdx(4, 3) + sq(4)*vdy(4, 3);dvpry(5, 4) = -vdz(4, 3);dvprz(5, 4) = -(sq(4)*vdx(4, 3)) + cq(4)*vdy(4, 3);dvsecx(5, 4) = 0.61*wprz(4);dvsecz(5, 4) = -0.61*wprx(4);dvterx(5, 4) = dvsecz(5, 4)*wy(4);dvtery(5, 4) = -(dvsecz(5, 4)*wprx(4)) + dvsecx(5, 4)*wprz(4);dvterz(5, 4) = -(dvsecx(5, 4)*wy(4));dvquax(5, 4) = 0.61*dwz(4);dvquaz(5, 4) = -0.61*dwx(4);vdx(5, 4) = dvprx(5, 4) + dvquax(5, 4) + dvterx(5, 4);vdy(5, 4) = dvpry(5, 4) + dvtery(5, 4);vdz(5, 4) = dvprz(5, 4) + dvquaz(5, 4) + dvterz(5, 4);

aprx(4) = -0.282*wprz(4);aprz(4) = 0.282*wprx(4);asecx(4) = -0.282*dwz(4);asecz(4) = 0.282*dwx(4);aterx(4) = aprz(4)*wy(4);atery(4) = -(aprz(4)*wprx(4)) + aprx(4)*wprz(4);aterz(4) = -(aprx(4)*wy(4));ax(4) = asecx(4) + aterx(4) + vdx(4, 4);ay(4) = atery(4) + vdy(4, 4);az(4) = asecz(4) + aterz(4) + vdz(4, 4);fex(4) = 19.52*ax(4);fey(4) = 19.52*ay(4);fez(4) = 19.52*az(4);wprx(5) = cq(5)*wprx(4) + sq(5)*wy(4);wprz(5) = sq(5)*wprx(4) - cq(5)*wy(4);wy(5) = ds(5) + wprz(4);dwprx(5) = ds(5)*wy(4);dwpry(5) = -(ds(5)*wprx(4));dwsecx(5) = dwprx(5) + dwx(4);dwsecy(5) = dwpry(5) + dwy(4);dwsecz(5) = dds(5) + dwz(4);dwx(5) = cq(5)*dwsecx(5) + dwsecy(5)*sq(5);dwz(5) = -(cq(5)*dwsecy(5)) + dwsecx(5)*sq(5);dvprx(5, 5) = cq(5)*vdx(5, 4) + sq(5)*vdy(5, 4);dvprz(5, 5) = sq(5)*vdx(5, 4) - cq(5)*vdy(5, 4);dvsecx(5, 5) = -0.113*wprz(5);dvsecz(5, 5) = 0.113*wprx(5);dvterx(5, 5) = dvsecz(5, 5)*wy(5);dvtery(5, 5) = -(dvsecz(5, 5)*wprx(5)) + dvsecx(5, 5)*wprz(5);dvterz(5, 5) = -(dvsecx(5, 5)*wy(5));dvquax(5, 5) = -0.113*dwz(5);dvquaz(5, 5) = 0.113*dwx(5);vdx(5, 5) = dvprx(5, 5) + dvquax(5, 5) + dvterx(5, 5);vdy(5, 5) = dvtery(5, 5) + vdz(5, 4);vdz(5, 5) = dvprz(5, 5) + dvquaz(5, 5) + dvterz(5, 5);dvprx(6, 5) = cq(5)*vdx(5, 4) + sq(5)*vdy(5, 4);dvprz(6, 5) = sq(5)*vdx(5, 4) - cq(5)*vdy(5, 4);dvsecx(6, 5) = -0.113*wprz(5);dvsecz(6, 5) = 0.113*wprx(5);dvterx(6, 5) = dvsecz(6, 5)*wy(5);dvtery(6, 5) = -(dvsecz(6, 5)*wprx(5)) + dvsecx(6, 5)*wprz(5);dvterz(6, 5) = -(dvsecx(6, 5)*wy(5));dvquax(6, 5) = -0.113*dwz(5);dvquaz(6, 5) = 0.113*dwx(5);vdx(6, 5) = dvprx(6, 5) + dvquax(6, 5) + dvterx(6, 5);vdy(6, 5) = dvtery(6, 5) + vdz(5, 4);vdz(6, 5) = dvprz(6, 5) + dvquaz(6, 5) + dvterz(6, 5);aprx(5) = 0.011*wprz(5) - 0.034*wy(5);apry(5) = 0.034*wprx(5);aprz(5) = -0.011*wprx(5);asecx(5) = -0.034*dwsecz(5) + 0.011*dwz(5);asecy(5) = 0.034*dwx(5);asecz(5) = -0.011*dwx(5);aterx(5) = -(apry(5)*wprz(5)) + aprz(5)*wy(5);atery(5) = -(aprz(5)*wprx(5)) + aprx(5)*wprz(5);aterz(5) = apry(5)*wprx(5) - aprx(5)*wy(5);ax(5) = asecx(5) + aterx(5) + vdx(5, 5);ay(5) = asecy(5) + atery(5) + vdy(5, 5);az(5) = asecz(5) + aterz(5) + vdz(5, 5);fex(5) = 5.79*ax(5);

fex(5) = 5.79*ax(5);fey(5) = 5.79*ay(5);fez(5) = 5.79*az(5);wprx(6) = cq(6)*wprx(5) + sq(6)*wy(5);wpry(6) = -(sq(6)*wprx(5)) + cq(6)*wy(5);wz(6) = ds(6) + wprz(5);dwprx(6) = ds(6)*wy(5);dwpry(6) = -(ds(6)*wprx(5));dwsecx(6) = dwprx(6) + dwx(5);dwsecy(6) = dwpry(6) + dwsecz(5);dwsecz(6) = dds(6) + dwz(5);dwx(6) = cq(6)*dwsecx(6) + dwsecy(6)*sq(6);dwy(6) = cq(6)*dwsecy(6) - dwsecx(6)*sq(6);dvprx(6, 6) = cq(6)*vdx(6, 5) + sq(6)*vdy(6, 5);dvpry(6, 6) = -(sq(6)*vdx(6, 5)) + cq(6)*vdy(6, 5);dvsecx(6, 6) = 0.103*wpry(6);dvsecy(6, 6) = -0.103*wprx(6);dvterx(6, 6) = -(dvsecy(6, 6)*wz(6));dvtery(6, 6) = dvsecx(6, 6)*wz(6);dvterz(6, 6) = dvsecy(6, 6)*wprx(6) - dvsecx(6, 6)*wpry(6);dvquax(6, 6) = 0.103*dwy(6);dvquay(6, 6) = -0.103*dwx(6);vdx(6, 6) = dvprx(6, 6) + dvquax(6, 6) + dvterx(6, 6);vdy(6, 6) = dvpry(6, 6) + dvquay(6, 6) + dvtery(6, 6);vdz(6, 6) = dvterz(6, 6) + vdz(6, 5);aprx(6) = -0.027*wpry(6);apry(6) = 0.027*wprx(6);asecx(6) = -0.027*dwy(6);asecy(6) = 0.027*dwx(6);aterx(6) = -(apry(6)*wz(6));atery(6) = aprx(6)*wz(6);aterz(6) = apry(6)*wprx(6) - aprx(6)*wpry(6);ax(6) = asecx(6) + aterx(6) + vdx(6, 6);ay(6) = asecy(6) + atery(6) + vdy(6, 6);az(6) = aterz(6) + vdz(6, 6);fex(6) = 1.69*ax(6);fey(6) = 1.69*ay(6);fez(6) = 1.69*az(6);fx(6) = -fex(6);fy(6) = -fey(6);fz(6) = -fez(6);pprx(6) = -0.001*dwx(6);ppry(6) = -0.001*dwy(6);pprz(6) = -0.001*dwsecz(6);psecx(6) = 0.001*wprx(6);psecy(6) = 0.001*wpry(6);psecz(6) = 0.001*wz(6);pterx(6) = -(psecz(6)*wpry(6)) + psecy(6)*wz(6);ptery(6) = psecz(6)*wprx(6) - psecx(6)*wz(6);pterz(6) = -(psecy(6)*wprx(6)) + psecx(6)*wpry(6);pquix(6) = 0.076*fey(6);pquiy(6) = -0.076*fex(6);px(6) = pprx(6) + pquix(6) + pterx(6);py(6) = ppry(6) + pquiy(6) + ptery(6);pz(6) = pprz(6) + pterz(6);fprx(5, 1) = cq(6)*fx(6) - fy(6)*sq(6);fpry(5, 1) = cq(6)*fy(6) + fx(6)*sq(6);fx(5) = -fex(5) + fprx(5, 1);fy(5) = -fey(5) + fpry(5, 1);fz(5) = -fez(5) + fz(6);

pprx(5) = -0.037*dwx(5);ppry(5) = -0.035*dwsecz(5);pprz(5) = -0.01*dwz(5);psecx(5) = 0.037*wprx(5);psecy(5) = 0.035*wy(5);psecz(5) = 0.01*wprz(5);pterx(5) = psecy(5)*wprz(5) - psecz(5)*wy(5);ptery(5) = psecz(5)*wprx(5) - psecx(5)*wprz(5);pterz(5) = -(psecy(5)*wprx(5)) + psecx(5)*wy(5);pquix(5) = -0.034*fey(5) - 0.102*fez(5);pquiy(5) = 0.034*fex(5);pquiz(5) = 0.102*fex(5);px(5) = 0.113*fz(6) + pprx(5) + pquix(5) + pterx(5) + cq(6)*px(6) - py(6)*sq(6);py(5) = ppry(5) + pquiy(5) + ptery(5) + cq(6)*py(6) + px(6)*sq(6);pz(5) = -0.113*fprx(5, 1) + pprz(5) + pquiz(5) + pterz(5) + pz(6);fprx(4, 1) = cq(5)*fx(5) + fz(5)*sq(5);fpry(4, 1) = -(cq(5)*fz(5)) + fx(5)*sq(5);fx(4) = -fex(4) + fprx(4, 1);fy(4) = -fey(4) + fpry(4, 1);fz(4) = -fez(4) + fy(5);pprx(4) = -0.818*dwx(4);ppry(4) = -0.0276*dwy(4) + 0.014*dwz(4);pprz(4) = 0.014*dwy(4) - 0.817*dwz(4);psecx(4) = 0.818*wprx(4);psecy(4) = -0.014*wprz(4) + 0.0276*wy(4);psecz(4) = 0.817*wprz(4) - 0.014*wy(4);pterx(4) = psecy(4)*wprz(4) - psecz(4)*wy(4);ptery(4) = psecz(4)*wprx(4) - psecx(4)*wprz(4);pterz(4) = -(psecy(4)*wprx(4)) + psecx(4)*wy(4);pquix(4) = 0.328*fez(4);pquiz(4) = -0.328*fex(4);px(4) = -0.61*fy(5) + pprx(4) + pquix(4) + pterx(4) + cq(5)*px(5) + pz(5)*sq(5);py(4) = ppry(4) + ptery(4) - cq(5)*pz(5) + px(5)*sq(5);pz(4) = 0.61*fprx(4, 1) + pprz(4) + pquiz(4) + pterz(4) + py(5);fprx(3, 1) = cq(4)*fx(4) - fz(4)*sq(4);fpry(3, 1) = cq(4)*fz(4) + fx(4)*sq(4);fprz(3, 1) = -fy(4);fx(3) = -fex(3) + fprx(3, 1);fy(3) = -fey(3) + fpry(3, 1);fz(3) = -fez(3) + fprz(3, 1);pprx(3) = 0.034*dwsecz(3) - 0.5360000000000001*dwx(3) - 0.05600000000000001*dwz(3);ppry(3) = -0.48*dwsecz(3) + 0.034*dwx(3) + 0.01*dwz(3);pprz(3) = 0.01*dwsecz(3) - 0.05600000000000001*dwx(3) - 0.4060000000000001*dwz(3);psecx(3) = 0.536*wprx(3) + 0.056*wprz(3) - 0.034*wy(3);psecy(3) = -0.034*wprx(3) - 0.01*wprz(3) + 0.48*wy(3);psecz(3) = 0.056*wprx(3) + 0.406*wprz(3) - 0.01*wy(3);pterx(3) = psecy(3)*wprz(3) - psecz(3)*wy(3);ptery(3) = psecz(3)*wprx(3) - psecx(3)*wprz(3);pterz(3) = -(psecy(3)*wprx(3)) + psecx(3)*wy(3);pquix(3) = -0.029*fey(3) + 0.018*fez(3);pquiy(3) = 0.029*fex(3) - 0.05800000000000001*fez(3);pquiz(3) = -0.018*fex(3) + 0.05800000000000001*fey(3);px(3) = pprx(3) + pquix(3) + pterx(3) + cq(4)*px(4) - pz(4)*sq(4);py(3) = 0.11*fprz(3, 1) + ppry(3) + pquiy(3) + ptery(3) + cq(4)*pz(4) + px(4)*sq(4);pz(3) = -0.11*fpry(3, 1) + pprz(3) + pquiz(3) + pterz(3) - py(4);fprx(2, 1) = cq(3)*fx(3) + fz(3)*sq(3);fpry(2, 1) = -(cq(3)*fz(3)) + fx(3)*sq(3);fx(2) = -fex(2) + fprx(2, 1);fy(2) = -fey(2) + fpry(2, 1);fz(2) = -fez(2) + fy(3);

pprx(2) = -1.022*dds(2) - 0.831*dwx(2);ppry(2) = -8.832*dwy(2);pprz(2) = -8.983*dds(2) - 1.022*dwx(2);psecx(2) = 1.022*ds(2) + 0.831*wprx(2);psecy(2) = 8.832*wpry(2);psecz(2) = 8.983*ds(2) + 1.022*wprx(2);pterx(2) = ds(2)*psecy(2) - psecz(2)*wpry(2);ptery(2) = -(ds(2)*psecx(2)) + psecz(2)*wprx(2);pterz(2) = -(psecy(2)*wprx(2)) + psecx(2)*wpry(2);pquix(2) = 0.146*fey(2);pquiy(2) = -0.146*fex(2) + 0.166*fez(2);pquiz(2) = -0.166*fey(2);px(2) = pprx(2) + pquix(2) + pterx(2) + cq(3)*px(3) + pz(3)*sq(3);py(2) = 0.61*fy(3) + ppry(2) + pquiy(2) + ptery(2) - cq(3)*pz(3) + px(3)*sq(3);pz(2) = -0.61*fpry(2, 1) + pprz(2) + pquiz(2) + pterz(2) + py(3);fprx(1, 1) = cq(2)*fx(2) - fy(2)*sq(2);fpry(1, 1) = cq(2)*fy(2) + fx(2)*sq(2);fx(1) = -fex(1) + fprx(1, 1);fy(1) = 479.5231800000001 + fpry(1, 1);fz(1) = -fez(1) + fz(2);pprx(1) = 0.314*dwy(1);ppry(1) = -0.731*dwy(1);pprz(1) = -0.099*dwy(1);psecx(1) = -0.314*wy(1);psecy(1) = 0.731*wy(1);psecz(1) = 0.099*wy(1);pterx(1) = -(psecz(1)*wy(1));pterz(1) = psecx(1)*wy(1);pquix(1) = 11.02903314 + 0.762*fez(1);pquiy(1) = 0.023*fex(1) + 0.07299999999999998*fez(1);pquiz(1) = 35.00519213999999 - 0.762*fex(1);px(1) = -0.85*fz(2) + pprx(1) + pquix(1) + pterx(1) + cq(2)*px(2) -py(2)*sq(2);py(1) = -0.15*fz(2) + ppry(1) + pquiy(1) + cq(2)*py(2) + px(2)*sq(2);pz(1) = 0.85*fprx(1, 1) + 0.15*fpry(1, 1) + pprz(1) + pquiz(1) + pterz(1) + pz(2);tau(1) = py(1) ;tau(2) = -pz(2);tau(3) = -py(3);tau(4) = py(4) ;tau(5) = -py(5);tau(6) = -pz(6);

However it is derived, the dynamic model is definetely complex….


Direct dynamicsFor given joint torques τ(t), determine the joint accelerations �̈�𝐪(𝑑𝑑) and, if initial positions 𝐪𝐪 𝑑𝑑0 and velocities �̇�𝐪 𝑑𝑑0 are known, the positions 𝐪𝐪 𝑑𝑑 and the velocities �̇�𝐪 𝑑𝑑 .

𝐁𝐁 𝐪𝐪 �̈�𝐪 + 𝐂𝐂 𝐪𝐪, �̇�𝐪 �̇�𝐪 + 𝐠𝐠 𝐪𝐪 + 𝐅𝐅𝑣𝑣�̇�𝐪 + 𝐟𝐟𝑠𝑠 𝐪𝐪, �̇�𝐪 = τ

Inverse dynamicsFor given accelerations �̈�𝐪(𝑑𝑑), velocities �̇�𝐪 𝑑𝑑 and positions 𝐪𝐪 𝑑𝑑 determine the joint torques τ(t) needed for motion generation.

Problem whose solution is useful in order to compute the simulation model of the robot manipulator It can be solved both with Euler-Lagrange and with Newton-Euler approaches

Problem whose solution is useful for trajectory planning and model based control It can be efficiently solved with the Newton-Euler formulation

Direct and inverse dynamics


𝐁𝐁 𝐪𝐪 �̈�𝐪 + 𝐧𝐧 𝐪𝐪, �̇�𝐪 = τ

As for the computation of the direct dynamics, let us rewrite the dynamic model of the manipulator in these terms:

𝐧𝐧 𝐪𝐪, �̇�𝐪 = 𝐂𝐂 𝐪𝐪, �̇�𝐪 �̇�𝐪 + 𝐠𝐠 𝐪𝐪 + 𝐅𝐅𝑣𝑣�̇�𝐪 + 𝐟𝐟𝑠𝑠 𝐪𝐪, �̇�𝐪

where:

Computation of the inverse dynamics can be easily done both with the Euler-Lagrange method and with the Newton-Euler one.

We thus have to numerically integrate the explicit system of differential equations:

�̈�𝐪 = 𝐁𝐁 𝐪𝐪 −1 τ − 𝐧𝐧 𝐪𝐪, �̇�𝐪

where all the elements needed to build the system are directly computed by the Euler-Lagrange method.

Computation of direct and inverse dynamics


With the current values of 𝐪𝐪 and �̇�𝐪, a first iteration of the script is performed, setting �̈�𝐪 = 0. In this way the torques τ computed by the method directly return the vector 𝐧𝐧.

How to compute the direct dynamics with the Newton-Euler method?

Then we set 𝐠𝐠0 = 0 inside the script (in order to eliminate the gravitational effects) and �̇�𝐪 = 0 (in order to eliminate Coriolis, centrifugal and friction effects). 𝑛𝑛 iterations of the script are performed, with �̈�𝑞𝑖𝑖 = 1 and �̈�𝑞𝑃𝑃 = 0, 𝑗𝑗 ≠ 𝑖𝑖. This way matrix 𝐁𝐁 is formed column by column and all elements to form the system of equations are available.

Newton-Euler script (Matlab, C, …):

τ = ΝΕ 𝐪𝐪, �̇�𝐪, �̈�𝐪

Computation of direct and inverse dynamics

Control of industrial robots Robot dynamics

Documents

Transcript of Control of industrial robots Robot dynamics