Elements of Kinematics and Dynamics of Industrial Robotscampus.unibo.it/218969/1/IRM_01_Kinematics_Dynamics.pdf · robot executes its tasks. It is the set of all the points (conﬁgurations)

IntroductionKinematic model of a manipulator

Dynamic model

Elements of Kinematics and Dynamics of Industrial Robots

Claudio Melchiorri

Dipartimento di Ingegneria dell’Energia Elettrica e dell’Informazione (DEI)

Universita di Bologna

email: [email protected]

Claudio Melchiorri Elements of Kinematics and Dynamics 1 / 106


Dynamic model

Summary

1 IntroductionDegrees of Freedom of a ManipulatorJoint Space and Work SpaceTypes of robotic manipulators

2 Kinematic model of a manipulatorForward kinematicsInverse KinematicsDifferential kinematics. The Jacobian

3 Dynamic modelNewton–EulerFinal considerations



Dynamic model

Summary






Dynamic model

Summary






Dynamic model

Degrees of Freedom of a ManipulatorJoint Space and Work SpaceTypes of robotic manipulators

IntroductionDegrees of Freedom of a Manipulator

A manipulator may be described as the interconnection of rigid bodies(links) through kinematic pairs (joints)

A joint is an element that constrains one or more relative motion directionsbetween two rigid bodies (links). In robotics, joints are of two types:

rotoidal: rotation about a fixed axisprismatic: translation along a fixed axis

Each joint is characterised by the number of independent motiondirections allowed between the two consecutive links. Such a numberdefines the degrees of freedom of the joint⇒ rotoidal and prismatic joints have 1 degree of freedom (dof)!



Dynamic model


IntroductionDegrees of Freedom of a Manipulator

If a joint has k degrees of freedom, then the relative configuration betweentwo rigid bodies may be expressed as a function of k variablesq1, q2, . . . , qk , called joint variables

rotoidal joint: q is the amplitude of the rotation (θ)prismatic joint: q is the amplitude of the translation (d)

Let us consider a manipulator with n + 1 links (L0, L1, . . . , Ln)interconnected by n joints (q1, q2, . . . qn), and indicate as ki the degrees offreedom of the i-th joint, i = 1, . . . , n⇒ we assume a serial kinematic chain!

The manipulator configuration depends on Ndof independent variables,where

Ndof =n∑

i=1

ki

Ndof represents the number of degrees of freedom (dof) of the manipulator⇒ Ndof also represents the number of actuators⇒ Ndof = n for a manipulator with n+1 links and rotoidal/prismatic joints



Dynamic model


IntroductionJoint Space and Work Space

Joint Space. Let us stack all the joint variables in a vector q = [q1, q2, . . . , qn]T ,q ∈ Q ⊂ R

Ndof . The set Q is called joint space, and contains all the possiblevalues that the joint variables may assume. Note that for each q ∈ Q, there is aunique configuration of the mechanical structure.

Work Space. The work space is a subset of the Euclidean space E in which therobot executes its tasks. It is the set of all the points (configurations) that themechanical structure may assume, and in general is a 3D (2D) subset of E. Eachpoint of the work space is indicated by a vector x of proper dimension,x ∈ R

y , y = {3, 6}.

Configuration of a manipulator. It takes into account both the position and theorientation of a reference frame fixed to the manipulator extremity (end effector).Then (locally):

x ∈ R3 in a plane;

x ∈ R6 in space

Classification of manipulators. If Rn is the joint space and Rm the work space:

n = m: “normal” casen < m: defective manipulatorsn > m: redundant manipulators



Dynamic model


IntroductionTypes of robotic manipulators

The first three dof define the structure of the robot.

By composing rotoidal and prismatic joints, it is possible to obtain differentkinematic structures, with different properties that can be exploited in differenttypes of tasks.

Among the kinematic structures that have been developed, we have:

Cartesian robots

Anthropomorphic robots

SCARA robots

Cylindrical and spherical robots

Spherical wrists



Dynamic model



Cartesian Manipulator



Dynamic model



Anthropomorphic Manipulator



Dynamic model



SCARA Manipulator



Dynamic model



Cylindrical Manipulator Spherical Manipulator



Dynamic model



spherical wrist



Dynamic model

Forward kinematicsInverse KinematicsDifferential kinematics. The Jacobian

Kinematic Model of Robot Manipulators

Forward kinematics

Inverse kinematics

Differential kinematics and Statics (Jacobian)



Dynamic model


Kinematic model

In describing the kinematics of a robot, there are two problems:

Direct (forward) kinematic model: once the position, velocity, accelerationvalues are known in the joint space, determine the corresponding values inthe work space (in a proper reference frame, e.g. Cartesian)

x = f (q) q ∈ Rn, x ∈ R

m

Inverse kinematic model: determine the inverse mapping of the kinematicvariables from the work space to the joint space

q = g(x) = f −1(x) q ∈ Rn, x ∈ R

m

It is possible to define different kinematic models for a given manipulator,although equivalent from a mathematical point of view.



Dynamic model


Kinematic modelExample: a 2 dof planar robot

Forward kinematic model:

x = l1 cos θ1 + l2 cos(θ1 + θ2)

y = l1 sin θ1 + l2 sin(θ1 + θ2)

φ = θ1 + θ2

An easy problem...

Inverse kinematic model:

cos θ2 =x20 + y20 − l21 − l22

2l1 l2

, sin θ2 = ±

√

(1 − cos2 θ2)

θ2 = atan2(sin θ2, cos θ2)

k1 = l1 + l2 cos θ2, k2 = l2 sin θ2

sin θ1 =y0k1 − x0k2

k21

+ k22

, cos θ1 =y0 − k1 sin θ1

k2

θ1 = atan2(sin θ1, cos θ1)

• The solution is not so simple.• Two possible solutions (sign of sin θ2).



Dynamic model


Kinematic modelDirect kinematic model

The homogeneous transformation matrixs iTj are used to define the kinematicmodel.

The configuration of a rigid body bi in 3D space may be described by thehomogeneous transformation 0Ti between a reference frame FFi (fixed to therigid body) and the base frame FF0

0Ti =

(0Ri

0pi

0 0 0 1

)

,(

0Ri

)T

=(

0Ri

)−1

, 0pi ∈ R3

R ∈ SO(3) is a (3× 3) rotation matrix, 0pi is a 3-dimensional position vector.



Dynamic model



Given two rigid bodies bi and bj with the corresponding matrixs 0Ti and0Tj ,

their relative configuration is given by:

jTi =(

0Tj

)−10Ti



Dynamic model



Homogeneous transformation matrixs iTj are used to define the kinematicmodel.

A manipulator is a mechanical system made of a chain of rigid bodies, thelinks, connected by joints. A reference frame is properly associated to eachlink, and two consecutive reference frames are related by the homogeneoustransformation matrix i−1Ti , that results to be function of the joint variable qi .

Links: L0, L1, . . . , Ln

Joints: J1, J2, . . . , Jn

Frames: F0,F1, . . . ,Fn

Transformation matrixs:0T1,

1T2, . . . ,n−2Tn−1,

n−1Tn

Kinematic model:0Tn = 0T1

1T2 . . . n−1Tn



Dynamic model



A notation to define the kinematic model (Denavit-Hartenberg).

Each link is numbered, from 0 to n, so that it is uniquely identified in themechanical chain: L0, L1, . . . , Ln. The base link is conventionally identifiedas L0, the distal link as Ln. A manipulator with n + 1 links has n joints,each of them allowing the relative motion between two consecutive links.Also joints are numbered, from 1 to n, starting from the base: J1, J2, . . . ,Jn. According to this definition, Ji connects the links Li−1 and Li .Frames Fi are associated to each joint, according to the specificDenavit-Hartenberg procedure, and matrixs i−1Ti are computed.

Links: L0, L1, . . . , Ln

Joints: J1, J2, . . . , Jn

Frames: F0,F1, . . . ,Fn

Transformation matrixs:0T1,

1T2, . . . ,n−2Tn−1,

n−1Tn

Kinematic model:0Tn = 0T1

1T2 . . . n−1Tn



Dynamic model



The motion of the joints changes the position and the orientation in spaceof the extremity of the manipulator. Then, the position and theorientation are (in general) non linear functions of the n joint variables q1,q2, . . . , qn, that is:

p = f (q1, q2, . . . , qn) = f (q)

whereq = (q1, q2, . . . , qn)

T is defined in the joint space Rn

p is defined in the work space Rm

Vector p usually takes into account:some components for the position (e.g. the position x , y , z , with respect toa Cartesian base frame)some components for orientation (e.g. Euler or RPY angles).

The Denavit-Hartenberg notation gives a systematic procedure to definethe kinematic model of a robot manipulator, and requires 4 parametersonly (not 6) for each link: di , θi , ai , αi .

Two constraints: 1) axis xi+1 intersects, and 2) it is perpendicular to zi .



Dynamic model


Kinematic modelDenavit-Hartenberg Parameters

Conclusion: the position and the orientation of two consecutive frames, andtherefore of the related links, may be defined by the four Denavit-Hartenbergparameters:

ai = length of the common normal between the axes of two consecutivejoints

αi = ccw angle between zi−1 the axis of joint i , and zi , axis of joint i + 1

di = distance between the origin oi−1 of Fi−1 and the point pi ,

θi = ccw angle about zi−1 between the axis xi−1 and the common normalxi between zi−1 and zi .

i−1Ti =

Cθi −SθiCαi SθiSαi aCθi

Sθi CθiCαi −CθiSαi aSθi0 Sαi Cαi di0 0 0 1



Dynamic model



Given two consecutive links Li−1 and Li connected by joint Ji , matrixi−1Ti is function of the joint variable qi , that is of a rotation θi if the jointis rotational or of a translation di if the joint is prismatic⇒ i−1Ti =

i−1Ti (qi)In case of a manipulator with n joints, the relationship between frames F0

and Fn is given by0Tn = 0T1(q1)

1T2(q2) · · ·n−1Tn(qn)

Once the joint variables q1, q2, . . . , qn are known, this equation definesposition and orientation of the last link (frame) with respect to the basereference frameThis equation is the kinematic model of the manipulator

It is always possible to derive the for-ward kinematic model (for any serial-chain manipulator).



Dynamic model



Example. Let us consider a planar, 2 dof robot

matrixs i−1Ti are defined as (Ci = cos θi , Si = sin θi):

0T1(θ1) =

C1 −S1 0 a1C1

S1 C1 0 a1S1

0 0 1 00 0 0 1

1T2(θ2) =

C2 −S2 0 a2C2

S2 C2 0 a2S2

0 0 1 00 0 0 1



Dynamic model



Then (Cij = cos(θi + θj), Sij = sin(θi + θj)):

0T2 =0T1

1T2 =

(n s a p0 0 0 1

)

=

C12 −S12 0 a1C1 + a2C12

S12 C12 0 a1S1 + a2S12

0 0 1 00 0 0 1

Vectors n, s, a define the orientation of the extremity of the manipulator(rotation about z), while p defines its position (plane x − y)



Dynamic model


Example: PUMA 260

Homogeneous transformation matrixs:

0T1 =

C1 0 S1 0S1 0 −C1 00 1 0 00 0 0 1

1T2

C2 −S2 0 a2C2S2 C2 0 a2S20 0 1 00 0 0 1

2T3 =

C3 0 S3 0S3 0 −C3 00 1 0 −d30 0 0 1

3T4 =

C4 0 −S4 0S4 0 C4 00 −1 0 d40 0 0 1

4T5 =

C5 0 S5 0S5 0 −C5 00 1 0 00 0 0 1

5T6 =

C6 −S6 0 0S6 C6 0 00 0 1 d60 0 0 1



Dynamic model


Example: PUMA 260. Matrix 0T6 =

[n s a p

0 0 0 1

]

, with

n =

S1(C5C6S4 + C4S6) + C1(C2(−(C6S3S5) + C3(C4C5C6 − S4S6)) − S2(C3C6S5 + S3(C4C5C6 − S4S6)))

−(C1(C5C6S4 + C4S6)) + S1(C2(−(C6S3S5) + C3(C4C5C6 − S4S6)) − S2(C3C6S5 + S3(C4C5C6 − S4S6)))

S2(−(C6S3S5) + C3(C4C5C6 − S4S6)) + C2(C3C6S5 + S3(C4C5C6 − S4S6))

s=

S1(C4C6 − C5S4S6)+C1(C2(S3S5S6 + C3(−(C6S4) − C4C5S6)) − S2(−(C3S5S6) + S3(−(C6S4) − C4C5S6)))

−(C1(C4C6 − C5S4S6))+S1(C2(S3S5S6 + C3(−(C6S4) − C4C5S6))−S2(−(C3S5S6)+S3(−(C6S4) − C4C5S6)))

S2(S3S5S6 + C3(−(C6S4) − C4C5S6)) + C2(−(C3S5S6)+S3(−(C6S4) − C4C5S6))

a =

S1S4S5 + C1(C2(C5S3 + C3C4S5) − S2(−(C3C5) + C4S3S5))

−(C1S4S5) + S1(C2(C5S3 + C3C4S5) − S2(−(C3C5) + C4S3S5))

S2(C5S3 + C3C4S5) + C2(−(C3C5) + C4S3S5)

p =

S1(−d3 + d6S4S5) + C1(a2C2 + C2((d4 + C5d6)S3 + C3C4d6S5) − S2(−(C3(d4 + C5d6)) + C4d6S3S5))

−(C1(−d3 + d6S4S5)) + S1(a2C2 + C2((d4 + C5d6)S3 + C3C4d6S5) − S2(−(C3(d4 + C5d6)) + C4d6S3S5))

a2S2 + S2((d4 + C5d6)S3 + C3C4d6S5) + C2(−(C3(d4 + C5d6)) + C4d6S3S5)



Dynamic model


Kinematic modelInverse Kinematics

It is always possible to compute the forward kinematic model with a standardprocedure (DH).

Viceversa, a standard procedure to obtain the inverse kinematic modelq = f −1(x) = g(x) does not exist!

Moreover, we may have:no solution (if x does not belong to the workspace)a finite set of solutions (in general more than one)infinite solutions.

We look for closed-form solutions, and not for numerical procedures:an analytical solution is better from the computational point of viewgiven the analytical expressions, it is easy to select a particular solution inthe set of solutions.



Dynamic model



Example: an anthropomorphic arm has 4 possible different configurations toreach a given point in space!



Dynamic model



A closed form solution to the inverse kinematic problem may be obtained, incase it exists, by trying one, or both, of these approaches:

ALGEBRAIC approach, that is an elaboration of the kinematic equationsin order to obtain a set of ‘simple’ equations that can be solved in theunknown joint variables q

GEOMETRIC approach, based, when possible, on considerationsdependent on the geometric structure of the manipulator, that may help insolving the problem.



Dynamic model



Algebraic Approach. The kinematic model of a 6 dof robot manipulator isgiven by the matrix equation

0T6 =0T1(q1)

1T2(q2) · · ·5T6(q6)

that is equivalent to 12 scalar equations in the 6 unknowns qi , i = 1, . . . , 6

Since the values of elements in 0T6 and the structure of matrixs i−1Ti areknown, pre- and post-multiplying we get:

[

0T1(q1) · · ·i−1Ti (qi)

]−10T6

[

jTj+1(qj+1) · · ·5T6(q6)

]−1

=

= iTi+1(qi+1) · · ·j−1Tj (qj)

that is, other 12 scalar equations for each pair (i , j), i < j

By selecting the most simple equations among those defined above, itmight be possible to find 6 equations that can be solved.⇒ this method is closely related to the expression of the direct kinematicmodel!



Dynamic model



Pieper approach

While the algebraic approach is not based on any general consideration thathelps in finding a solution, the geometric approach may give useful indicationsfor solving the inverse kinematics.

For several kinematic structures of industrial manipulators, it is possible toapply the kinematic decoupling principle, that allows to decompose theoverall problem into two, simpler, sub-problems:1. solution of the inverse kinematic problem for the position2. solution of the inverse kinematic problem for the orientation of the

end-effector

Pieper Approach: Sufficient condition to solve the inverse kinematicproblem for a manipulator with 6 degrees of freedom is that its geometricstructure has:

three consecutive rotational joints whose axes intersect in a point (sphericalwrist)

orthree consecutive rotational joints with parallel axes



Dynamic model



Spherical wrist (NB: the type/position of the first three joints q1, q2 e q3 doesnot matter!)

The end effector posi-tion/orientation is given in

0T6 =

(

R p

0 0 0 1

)

the vector pp can be computed from 0T6 (matrixs R, p): pp = p − d6z6being d6 a constant parameter, and z6 the last column of R⇒ pp is function of q1, q2 and q3 only!

given pp, the inverse kinematics is solved for q1, q2 and q3the rotation matrix 0R3 (function of the first three joints) is computed

since 0R33R6 = R, we have 3R6 =

0R−13 R = 0RT

3 R

given 3R6, the IK for the orientation is solved (Euler ang.): q4, q5, q6



Dynamic model


Kinematic modelDifferential kinematics. The Jacobian

In robotics, besides the relationship between joint positions and end-effectorposition/orientation, it is of interest to define also the relationships between:

end-effector velocity and joint velocities:(

vω

)

⇐⇒ q

force applied on the environment by the manipulator and correspondingjoint torques:

(fn

)

⇐⇒ τ

These two relationships are based on a matrix, defining therefore a linearmapping between joint- and work-space, known as the Jacobian of themanipulator. The Jacobian matrix, very important in robotics, is used also for:

studying the singularities of the mechanical structure;

defining numerical algorithms to solve the inverse kinematics;

studying the manipulability properties.



Dynamic model


Kinematic model

The six-dimensional velocity vector (twist) in the work-space is defined withthree linear and three rotational components:

x =

(vω

)

Two different (although equivalent) expressions of the Jacobian matrix can bedefined:

the analytic expression, used when the rotational component in x isdefined as γ, the time-derivative of a triple of orientation variables

the geometric expression, used when the rotational component in x isdefined as ω, the rotational velocity vector

It is possible to relate the two expressions by means of a proper matrix T (γ)

JG (q) = T (γ)JA(q) =

[I3 0

0 T (γ)

]

JA(q)



Dynamic model



Analytic expression

The analytic expression of the Jacobian is obtained by differentiating thesix-dimensional vector x = f (q) = [pT , γT ]T giving the position andorientation of the end-effector in the work-space with respect to the baseframe FF0; usually, γ containes the Euler or the RPY angles.

By differentiating f (q) we get:

dx =∂f

∂q(q)dq = J(q) dq ( x = J(q)q )

where the m × n matrix

J(q) =

∂f1∂q1

∂f1∂q2

· · · ∂f1∂qn

......

. . ....

∂fm∂q1

∂fm∂q2

· · · ∂fm∂qn

J(q) ∈ Rm×n

is called the Jacobian matrix, or simply the Jacobian, of the manipulator



Dynamic model



Example. Let us consider the 2 dof planar manipulator

The forward kinematics is given by:

px = a1 cos θ1 + a2 cos(θ1 + θ2)

py = a1 sin θ1 + a2 sin(θ1 + θ2)

γ = θ1 + θ2

Therefore:

pxpyγ

=

−a1 sin θ1 − a2 sin(θ1 + θ2) −a2 sin(θ1 + θ2)a1 cos θ1 + a2 cos(θ1 + θ2) a2 cos(θ1 + θ2)

1 1

︸︷︷︸

J(q)

(θ1θ2

)

︸︷︷︸

q



Dynamic model



Geometric expression

The geometric expression of the Jacobian is obtained by considering asrotational components of the velocity vector x the rotational vector ω, and notthe derivative of a triple of angles γ.

x =

[pγ

]

⇐⇒ x =

[pω

]

Note that, from a geometric point of view, the expression x = J(q)q impliesthat the vector x is a linear combination of the elements in q, where theweights are the columns Ji of matrix J(q) (weight of the joint velocity qi on x).

v = Jv1q1 + Jv2q2 + . . .+ Jvnqn

ω = Jω1q1 + Jω2q2 + . . .+ Jωnqn



Dynamic model



It is possible to proof that the generic i-th column of the geometric Jacobian isdefined as

[JviJωi

]

=

[0zi−1 × (0pn −

0pi−1)0zi−1

]

rotational joint

[JviJωi

]

=

[0zi−1

0

]

prismatic joint

It is simple to obtain these expressions once the forward kinematics of themanipulator is known, 0Tn = 0T1(q1)

1T2(q2) · · ·n−1Tn(qn), since the vectors

0pi and0zi are obtained from matrices 0Ti .

From a computational point of view, there is just a minor increase with respectto the computation of the forward kinematic model!



Dynamic model



Force domain

By exploiting the virtual work principle, from the equation x = J(q)q, thatgives the mapping in terms of velocities between the joint- and work-space, it ispossible to obtain an analogous equation in the force domain.

Since the work, that is the product of an applied external force with thedisplacement, is invariant with respect to the frame where it is computed,we have

wTdx = τTdq

w =(f T nT

)Tis a 6-dimensional vector (wrench) collecting the linear

forces f and the torques n applied to the manipulator

τ is the n-dimensional vector of the joint forces/torques



Dynamic model



From wTdx = τTdq, since

dx = J(q)dq

we easily obtainτ = JT(q)w

This equation gives the relationship between the joint torque vector τ and theforce/torque vector w applied by the manipulator in the work-space.



Dynamic model



Singularities: The equation x = J(q)q may be interpreted, from aninfinitesimal point of view, as:

dx = J(q)dq

that is as a relationship between infinitesimal displacements in Rn and R

6. Ingeneral, rank J(q) = min(6, n), but:

⇒ there are joint configurations q in which J(q) looses rank(rank J(q) < min(6, n))

⇒ in these configurations, there are “directions” in R6 without a

corresponding “direction” in Rn (the robot cannot move along these

directions)

In singular configurations:

some motion directions in R6 cannot be achieved

limited velocities of the manipulator end-effector may correspond toinfinite velocities in the joint space

usually, they correspond to points at the border of the work-space, that isto points of maximum extension of the manipulator

a well defined solution to the inverse kinematic problem does not exist:either no solution or infinite solutions



Dynamic model



Example. Let us consider the Jacobian matrix

J(q) =

(1 10 0

)

, with rank J(q) = 1

For any value of dq1 and dq2 we have:{

dx1 = dq1 + dq2

dx2 = 0

⇒ Any vector dx with dx2 6= 0 is not physically achievable, ∀ dq1,dq2

Because of the duality of velocities and forces, we have

τ =

(τ1τ2

)

= JTf =

(1 01 0

)(fxfy

)

=

(fxfx

)

⇒ force fy does not affect joint torques τ1, τ2



Dynamic model



J(q) =

(−a1S1 − a2S12 −a2S12

a1C1 + a2C12 a2C12

)

N.B.: det{J(q)} = a1a2sin(θ2)

If θ1 = θ2 = 0, we have

J(q) =

(0 02 1

)

, a1 = a2 = 1

Then {

dx = 0

dy = 2dq1 + dq2e

{

τ1 = 2fy

τ2 = fy

⇒ no (instantaneous) motion is possible along x

⇒ forces along x do not ‘generate’ joint torques (can be applied without any“effort” at joints)



Dynamic model



Inverse differential kinematicsThe forward mapping between joint-a and work-space is given by

x = J(q)q, J(q) ∈ Rm×n

The inverse mapping is then given as the solution of a linear (matrix) algebraicequation.

m = n: if the manipulator is not in a singular configuration, it is possibleto use the inverse of the Jacobian:

q = J−1(q)x

m 6= n: the Moore-Penrose pseudoinverse of the Jacobian is used:

q = J+(q)x

J+ = JT(JJT

)−1

if m < n (right pseudoinverse)

JJ+ = Im

J+ =(JTJ

)−1JT

if m > n (left pseudoinverse)

J+J = In

Ip: (p × p) identity matrix



Dynamic model



n 6= m

rank J = p < min(m, n)

The solution given by the pseudoinverse qs = J+x is such that (xs = Jqs):

{‖x − xs‖ the norm of the error is minimum

‖qs‖ the norm of the solution is minimum



Dynamic model



Solution of x = Jq if m < n

• rank(J) = min(m, n) = m → R(J ) = IRm

• ∀ x ∃ q tale che x = Jq (more than one exists!)

• q = J+x ∃ N (J) such that ∀ q ∈ N (J) → x = J q = 0

→ q = J+x+ qN → x = J (J+x+ qN) = x, ∀qN ∈ N (J )

→ q = J+x + (I − J+J)y general expression of the solution(I − J+J) is a base of N (J)

• q = J+x has minimum norm



Dynamic model



The Jacobian may be used to solve the inverse kinematic problem if aclosed form solution is not available

A method could be to compute

qk+1 = qk + J−1(qk)vk dt

where a numerical integration of the position is performed

Unfortunately, this method is subject to numerical drifts and initializationproblems, and therefore it is difficult to obtain the desired solution

An alternative method is based on a feedback loop, by defining an error inthe work-space. Given

e = xd − x

By differentiating, we obtain

e = xd − J(q)q

⇒ we have to define q in such a way that e converges to 0 (e → 0 whent → ∞)



Dynamic model



Algorithm 1: Ifq = J−1(q)(xd + Ke)

with K = KT > 0, thene = −Ke

⇒ Inverse kinematic algorithm based on the Jacobian (pseudo-)inverse



Dynamic model



Algorithm 2: Given xd = 0, if

q = JT(q)Ke

with K = KT > 0, then

V (e) =1

2eTKe ⇒ V (e) = eTKe = −eTKJ(q)JT(q)Ke ≤ 0

⇒ Inverse kinematic algorithm based on the Jacobian transpose

⇒ q = 0 with e 6= 0 if Ke 6∈ ker JT(q)



Dynamic model



Manipulability measures

The Jacobian may be used to evaluate the achievable performances of amanipulator, in terms of velocities and applicable forces, or the predispositionof a manipulator to execute a given task⇒ manipulability ellipsoids



Dynamic model


Velocity manipulability ellipsoids

Velocity. Given a sphere with unit radius in the joint velocity space,qTq = 1, that may be considered as a “cost” (energy in input to themanipulator), we want to obtain its equivalent expression in the work-space

From x = J(q)q we obtain:

xT

(

JJT

)+

x = 1

that is an ellipsoid in the work-space Rm:

the directions of the principal axes are given by the eigenvectors of JJT

the length of the principal axes are given by the singular values of J,

σi =√

λi (JJT)



Dynamic model



Force manipulability ellipsoids

Force. Let us consider now the equation τTτ = 1, from which we obtain:

wTJJTw = 1

This equation describes an ellipsoids in the Rm force space

direction of the principal axes given by the eigenvectors of JJT

length of the principal axes given by the inverse of the singular values of J

This result confirms the duality of the velocity/force spaces: along thedirections in which high velocity performances are obtained, low forceperformances are achieved, and viceversa.

Considerations:⇒ actuation requires a “large” amplification, and results better along

directions where the “eigenvectors” (eigenvalues) are larger⇒ control requires a “small” amplification, and results better along directions

where the “eigenvectors” (eigenvalues) are smaller (better “sensitivity”)⇒ the “optimal” actuation direction in the velocity (force) domain is also the

“best” control direction in the force (velocity) domain



Dynamic model



Velocity Ellipsoids

0 0.5 1 1.5 2

−1

−0.5

0

0.5

1

[m]

[m]

Force Ellipsoids

0 0.5 1 1.5 2

−1

−0.5

0

0.5

1

[m]

[m]



Dynamic model

Newton–EulerFinal considerations

Dynamic Model of Robot Manipulators

Euler-Lagrange model

Newton- Euler model



Dynamic model


Dynamic model

Dynamics of a robot manipulator: analysis of the relationship between theapplied forces/torques and the resulting motion of a robot (x = f /m)

For the dynamics, it is possible to define two “models”:Direct model: given the forces/torques applied at joints, the joint positionand velocity, compute the acceleration

q = f (q, q, τ)

and then

q =

∫

qdt q =

∫

qdt

Inverse model: given the desired joint acceleration, velocity and position,compute the corresponding force/torque to be applied

τ = f −1(q, q, q) = g(q, q, q)

In deriving the dynamic model of a robot, we refer to a chain of rigidbodies mutually, and ideally, connected among them.



Dynamic model


Dynamic model

The dynamic model of a robot is used for several purposes:

simulation: testing motion behaviour without resorting to physicalexperiments

analysis and synthesis of control laws

analysis of the structural properties of a manipulator in the design phase.

Two techniques are available for the computation of the dynamic model:

⇒ Euler-Lagrange approach.First approach to be developed. The dynamic model obtained in this manner issimpler and more intuitive, and also more suitable to understand the effects ofchanges in the mechanical parameters. The links are considered altogether, andthe model is obtained analytically. Drawbacks: the model is obtained startingfrom the kinetic and potential energies (non intuitive); the model is notcomputationally efficient.

⇒ Newton-Euler approach.

Based on a computationally efficient recursive technique that exploits the serial

structure of an industrial manipulator. On the other hand, the mathematical

model is not expressed in closed form.

Obviously, the two techniques are equivalent (provide the same results).



Dynamic model



From physics, we know that it is possible to define:

1 The Kinetic Energy function K(q, q)

2 The Potential Energy function P(q)

and therefore the Lagrangian function L(q, q) = K(q, q)− P(q)

The Euler-Lagrange equations for a system described by n Lagrangiancoordinates qi are

ψi =d

dt

(∂L

∂qi

)

−∂L

∂qii = 1, . . . , n

being ψi the non-conservative (external or dissipative) generalized forcesperforming work on qi . In robotics, we consider:

τi joint actuator torque[

JTFc]

iterm due to external (contact) forces

dii qi joint friction torque

Therefore: ψi = τi +[JTFc

]

i− dii qi .



Dynamic model



Since the potential energy does not depend on the velocity, the Euler-Lagrangeequations can be rewritten as

ψi =d

dt

(∂K

∂qi

)

−∂K

∂qi+∂P

∂qii = 1, . . . , n (1)

This formulation is more convenient since in robotics it is possible to computequite easily the terms K and P from the geometric properties of themanipulator. Then, by applying (1), the dynamic model is obtained.

Note that

K =n∑

i=1

Ki P =n∑

i=1

Pi



Dynamic model



Computation of the Kinetic Energy. For a rigid body B:

The mass can be computed by

m =

∫

B

ρ(x , y , z) dx dy dz

where ρ(x , y , z) is the mass density (assumed constant): ρ(x , y , z) = ρ.

The center of mass (CoM) is

pC =1

m

∫

B

p(x , y , z)ρ dx dy dz =1

m

∫

B

p(x , y , z) dm

The kinetic energy results as

K =1

2

∫

B

vT (x , y , z)v(x , y , z)ρ dx dy dz

=1

2

∫

B

vT (x , y , z)v(x , y , z) dm



Dynamic model



Let assume that vC and ω, i.e. thetranslational and rotational velocities ofthe center of mass, are known with re-spect to an inertial frame F0 .

The velocity of a generic point p′ of the body is

v = vC + ω × (p′ − pC ) = vC + ω × r (2)

Note that the velocity expressed in a frame F ’ (e.g. fixed to the rigid body)may be computed by introducing the rotation matrix R between F ’ and F0

RTv = RT (vC + ω × r) = RTvC + (RTω)× (RT r)

Thereforev′ = v′C + ω′ × r′



Dynamic model



Since the product ω × r in (2) can be expressed as S(ω) r, we have:

K =1

2

∫

B

vT (x , y , z)v(x , y , z) dm

=1

2

∫

B

(vC + Sr)T (vC + Sr) dm

=1

2

∫

B

vTC vC dm +1

2

∫

B

rTSTSr dm +

∫

B

vTC Sr dm

=1

2

∫

B

vTC vC dm +1

2

∫

B

rTSTSr dm

As a matter of fact, from the definition of CoM (∫

B r dm =∫

B(pC − p)dm = 0):

∫

B

vTC Sr dm = vTC S

∫

B

r dm = 0



Dynamic model



In conclusion

K =1

2

∫

B

vTC vC dm +1

2

∫

B

rTSTSr dm

The first term depends on the linear velocity vC of the center of mass

1

2

∫

B

vTC vC dm =1

2m vTC vC

For the second term, considering that aTb = Tr(a bT ) and the particularstructure of matrix S, we have

1

2

∫

B

rTSTSr dm =1

2

∫

B

Tr(Sr rTST ) dm =1

2Tr(S

∫

B

rrT dm ST )

=1

2Tr(S J ST ) =

1

2ω

T I ω I : body inertia matrix

Also this term depends on the velocity of the center of mass (in this case ω).



Dynamic model



Matrix J, Euler matrix, and matrix I, body inertia matrix, are symmetric, andhave the following general expressions:

J =

∫r2x dm

∫rx ry dm

∫rx rz dm∫

rx ry dm∫r2y dm

∫ry rz dm∫

rx rz dm∫ry rz dm

∫r2z dm

I =

∫(r2y + r2z ) dm −

∫rx ry dm −

∫rx rz dm

−∫rx ry dm

∫(r2x + r2z ) dm −

∫ry rz dm

−∫rx rz dm −

∫ry rz dm

∫(r2x + r2y ) dm

The elements of both matrices J and I depend on vector r, i.e. on the positionof the generic point of the i-th link with respect to its center of mass, definedin the base frame F0 .

Since the position of the i-th link depends on the configuration of themanipulator, matrices J and I are in general functions of the joint variables q!



Dynamic model



In conclusion, the kinetic energy of a rigid body is defined as (Konig Theorem)

K =1

2m vTC vC +

1

2ωT Iω (3)

Both terms depend only on the velocity of the rigid body.

The first term, being related to the magnitude of a vector (‖vC‖ = vTC vC ), is invariant withrespect to the reference frame used to express the velocity:

vTC vC = (RvC )T (RvC ) = vTC (R

TR)vC , ∀ R

This property holds also for the second term: the product ωT Iω is invariant with respect to thereference frame. As a matter of fact, the body inertia matrix is transformed according to thefollowing relation:

I′= R I R

T

Then: ωT Iω = ω

′T I′ω′ = (R ω)T (RIRT )(Rω) = ωT (RTR)I(RTR)ω.

Therefore, being (3) invariant with respect to the reference frame, F can be chosen in order tosimplify the computations required for the evaluation of K .



Dynamic model



Since the kinetic energy Ki of the generic i-th link is invariant with respect tothe reference frame (used to express vC ,ω, I), it is convenient to chose a frameFi fixed to the link itself, with origin in the center of mass.

In this manner matrix I is constant and simple to be computed on the basis ofthe geometric properties of the link.

On the other hand, normally the rotational velocity ω is defined in the baseframe F0 , and therefore it is needed to transform it according to RT

ω, beingR the rotation matrix between Fi and F0 (known from the kinematic model ofthe manipulator).



Dynamic model



In conclusion, the kinetic energy of a manipulator can be determined when, foreach link, the following quantities are known:

the link mass mi ;

the inertia matrix Ii , computed wrt a frame Fi fixed to the center of massin which it has a constant expression Ii ;

the linear velocity vCi of the center of mass, and the rotational velocity ωi

of the link (both expressed in F0 );

the rotation matrix Ri between the frame fixed to the link and F0 .

The kinetic energy Ki of the i-th link has the form:

Ki =1

2miv

TCivCi +

1

2ωTi Ri IiR

Ti ωi

It is now necessary to compute the linear and rotational velocities (vCi and ωi)as functions of the Lagrangian coordinates (i.e. the joint variables q).



Dynamic model



The end-effector velocity may be computed as a function of the joint velocitiesq1, . . . , qn through the Jacobian matrix J. The same methodology can be used tocompute the velocity of a generic point of the manipulator, and in particular thevelocity vCi = pCi of the center of mass pCi , that results function of the jointvelocities q1, . . . , qi only:

pCi = Jiv1q1 + Jiv2q2 + . . .+ Jivi qi = Jiv q

ωi = Jiω1q1 + Jiω2q2 + . . .+ Jiωi qi = Jiωq

where

Jiv =[

Jiv1 . . . Jivi 0 . . . 0]

Jiω =[

Jiω1 . . . Jiωi 0 . . . 0]

with (j = 1, . . . , i)[

JivjJiωj

]

=

[

zj−1 × (pCi − pj−1)zj−1

]

rotational joint

[

JivjJiωj

]

=

[

zj−1

0

]

linear joint

being pj−1 the position of the origin of the frame associated to the j-th link.



Dynamic model



In conclusion, for a n dof manipulator we have:

K =1

2

n∑

i=1

mivTCivCi +

1

2

n∑

i=1

ωTi Ri IiR

Ti ω

=1

2qT

n∑

i=1

[

miJi Tv (q)Jiv (q) + Ji T

ω (q)Ri IiRTi J

iω(q)

]

q

=1

2qTM(q)q

=1

2

n∑

i=1

n∑

j=1

Mij (q)qi qj

where –because of its definition– M(q) is a n × n, symmetric and positivedefinite matrix, function of the manipulator configuration q.

Matrix M(q) is called the Inertia Matrix of the manipulator.



Dynamic model



Computation of the Potential Energy. For rigid bodies, the only potentialenergy taken into account in the dynamics is due to the gravitational field g.For the generic i-th link

Pi =

∫

Li

gTp dm = gT

∫

Li

p dm = gTpCimi

The potential energy does not depend on the joint velocities q, and may beexpressed as a function of the position of the centers of mass. For the wholemanipulator:

P =n∑

i=1

gTpCimi

In case of flexible link, one should consider also terms due to elastic forces.

Important result: K e P are computed (once mi and I are known) with a proceduresimilar to the one adopted for the forward kinematic model:

• K → matrices Ji e Ri , • P → pCi position of the centers of mass



Dynamic model



Once K and P are known, it is possible to compute the dynamic model of themanipulator. The dynamics is expressed by

ψk =d

dt

(

∂L

∂qk

)

−∂L

∂qkk = 1, . . . , n

The Lagrangian function is

L = K − P =1

2

n∑

i=1

n∑

j=1

Mij qi qj −n∑

i=1

gTpCimi

Then∂L

∂qk=∂K

∂qk=

n∑

j=1

Mkj qj

and

d

dt

∂L

∂qk=

n∑

j=1

Mkj qj +n∑

j=1

d Mkj

dtqj =

n∑

j=1

Mkj qj +n∑

i=1

n∑

j=1

∂Mkj

∂qiqi qj

Moreover∂L

∂qk=

1

2

n∑

i=1

n∑

j=1

∂Mij

∂qkqi qj −

∂P

∂qk



Dynamic model



The Lagrangian equations have the following formulation

n∑

j=1

Mkj qj +n∑

i=1

n∑

j=1

[

∂Mkj

∂qi−

1

2

∂Mij

∂qk

]

qi qj +∂P

∂qk= ψk k = 1, . . . , n

By defining the term hkji(q) as

hkji (q) =∂Mkj (q)

∂qi−

1

2

∂Mij(q)

∂qk

and gk(q) as

gk (q) =∂P(q)

∂qk

the following equations are finally obtained

n∑

j=1

Mkj (q)qj +n∑

i=1

n∑

j=1

hkji (q)qi qj + gk (q) = ψk k = 1, . . . , n



Dynamic model



The elements Mkj (q), hijk(q), gk(q) are function of the joint position only, andtherefore their computation is relatively simple once the manipulator’s configuration isknown. They have the following physical meaning:

For the acceleration terms:

Mkk is the moment of inertia about the k-th joint axis, in a given configurationand considering blocked all the other joints

Mkk is function of the joint positions qk+1, . . . , qn only (NOT of q1, . . . , qk)

Mkj is the inertia coupling, accounting the effect of acceleration of joint j onjoint k

For the quadratic velocity terms:

hkjj q2j represents the centrifugal effect induced on joint k by the velocity of joint

j (notice that hkkk = ∂Mkk∂qk

= 0)

hkji qi qj represents the Coriolis effect induced on joint k by the velocities of jointsi and j

For the configuration-dependent terms:

gk represents the torque generated on joint k by the gravity force acting on themanipulator in the current configuration.



Dynamic model



Recalling that the non-conservative forces ψk are in general composed by

τk joint actuator torque[

JTFc]

kexternal (contact) forces

dkk qk joint friction torque

the Lagrangian equations

n∑

j=1

Mkj (q)qj +n∑

i=1

n∑

j=1

hkji (q)qi qj + gk (q) = ψk k = 1, . . . , n

can be written in matrix form as

M(q)q+ C(q, q)q+Dq+ g(q) = τ + JT (q)Fc

This matrix equation is known as the dynamic model of the manipulator.



Dynamic model


Euler-Lagrange model - Some considerations

The product C(q, q)q =∑n

i=1

∑nj=1 hkji (q)qi qj is a (n × 1) vector v whose

elements are quadratic functions of the joint velocities qj .

The k-th element vk of this vector is:

vk =

n∑

j=1

Ckj qj

where the elements Ckj are computed as

Ckj =n∑

i=1

cijk qi

with

cijk =1

2

[∂Mkj

∂qi+∂Mki

∂qj−∂Mij

∂qk

]

Christoffel Symbols (4)



Dynamic model



The elements of matrix C(q, q) are computed as follows. From

n∑

i=1

n∑

j=1

hkji qi qj =

n∑

i=1

n∑

j=1

[

∂Mkj

∂qi−

1

2

∂Mij

∂qk

]

qi qj

by exchanging the sum (i , j) and exploiting the symmetry one obtains

n∑

i=1

n∑

j=1

∂Mkj

∂qi=

1

2

n∑

i=1

n∑

j=1

[

∂Mkj

∂qi+∂Mki

∂qj

]

and thenn∑

i=1

n∑

j=1

[

∂Mkj

∂qi−

1

2

∂Mij

∂qk

]

=n∑

i=1

n∑

j=1

1

2

[

∂Mkj

∂qi+∂Mki

∂qj−∂Mij

∂qk

]

=n∑

i=1

n∑

j=1

cijk

where cijk = 12

[

∂Mkj

∂qi+ ∂Mki

∂qj−

∂Mij

∂qk

]

are the so-called Christoffel Symbols.

Since matrix M(q) is symmetric, for a given k then cijk = cjik .In conclusion, the elements of matrix C(q, q) are computed as

[C(q, q)]k,j =n∑

i=1

cijk qi (5)



Dynamic model



This is not the only possible expression for matrix C(q, q). In general, anymatrix such that

n∑

j=1

cij qj =

n∑

j=1

n∑

k=1

hijk qk qj

can be considered. The choice (4) is preferred since in this case the followingproperty is verified.

Property. Matrix N(q, q), defined as

N(q, q) = M(q, q)− 2C(q, q) (6)

in which the elements of C(q, q) are defined as

cijk =1

2

[∂Mkj

∂qi+∂Mki

∂qj−∂Mij

∂qk

]

[C(q, q)]k,j =n∑

i=1

cijk qi

results skew-symmetric, i.e. nkj = −njk , nkk = 0.



Dynamic model



In fact, by considering the generic element nkj , one obtains

nkj =d Mkj

dt− 2[C]kj

=n∑

i=1

[∂Mkj

∂qi− (

∂Mkj

∂qi+∂Mki

∂qj−∂Mij

∂qk)

]

qi

=n∑

i=1

[∂Mij

∂qk−∂Mki

∂qj

]

qi

from which it follows (if indices k and j are exchanged, because of thesymmetry of M(q)) that nkj = −njk .

Since matrix N is skew-symmetrix, then

xT N(q, q) x = 0, ∀x



Dynamic model



The conditionxT N(q, q) x = 0, ∀x

holds since N(q, q) is skew-symmetric, due to the particular choice of the elements ofmatrix C(q, q).On the other hand, the condition (with x = q)

qT N(q, q) q = 0

holds for any choice of matrix C(q, q) (from the energy conservation principle).

The evolution over time of the kinetic energy K must be equal to the work generatedby the forces acting at joints:

d K

dt=

1

2

d

dt

(

qTMq)

= qT (τ −Dq− g(q) − JTF)

The first element is (from the dynamic model Mq = −Cq−Dq− g(q) + τ − JTF):

1

2

d

dt

(

qTMq)

=1

2qT Mq+ qTMq =

1

2qT Mq+ qT (−Cq− Dq− g(q) + τ − JTF)



Dynamic model



Then

1

2qTMq+ qT (−Cq−Dq − g(q) + τ − JTF) = qT (τ −Dq− g(q)− JTF)

from which

1

2qTMq = qTCq =⇒ qT (M− 2C)q = 0

This equation holds ∀q and without any assumption on matrix C(q, q) (that is,it holds even if C is not based on the Cristoffel symbols).



Dynamic model



In deriving the dynamic model, the actuation system has not been taken intoaccount. This is normally composed by:

motors

reduction gears

trasmission system.

The actuation system has several effects on the dynamics:

if motors are installed on the links, then masses and inertia are changed

it introduces its own dynamics (electromechanical, pneumatic, hydraulic,...) that may be non negligible (e.g. in case of lightweight manipulators)

it introduces additional nonlinear effects such as backslash, friction,elasticity, ...

Notice that these effects could be considered by introducing suitable terms inthe dynamic model derived on the basis of the Euler-Lagrangian formulation.



Dynamic model


Example - 1

Dynamic model of a pendulum (one dof manipulator).

Consider

θ joint variable,

τ joint torque,

m mass,

L distance between center of mass and joint,

d viscous friction coefficient,

I inertia seen at the rotation axis.

Kinetic energy: K = 12I θ2

Potenzial energy: P = mgL(1− cos θ)

Lagrangian function L: L = 12I θ2 −mgL(1− cos θ)



Dynamic model


Example - 1

Lagrangian function: L = 12I θ2 −mgL(1− cos θ)

from which

∂L

∂θ= I θ,

d

dt

∂L

∂θ= I θ,

∂L

∂θ= −mgL sin θ

The generalized Lagrangian force in this case must account for the torqueapplied to the joint and for the friction effect:

ψ = τ − d θ

From the general expression

ψ =d

dt

(∂L

∂θ

)

−∂L

∂θ

we have the following second order differential equation

I θ + d θ +mgL sin θ = τ



Dynamic model


Example - 2

Dynamic model of a 2 dof manipulator. Consider:

θi i-th joint variable;

mi i-th link mass ;

Ii i-th link inertia, about an axis throughthe CoM and parallel to z;

ai i-th link length;

aCi distance between joint i and the CoMof the i-th link;

τi torque on joint i;

g gravity force along y;

Pi , Ki potential and kinetic energy of thei-th link.



Dynamic model


Example - 2

The dynamic model M(q)q+ C(q, q)q+Dq+ g(q) = τ results

M11θ1 +M12 θ2 + c121 θ1θ2 + c211θ2θ1 + c221 θ22 + g1 = τ1

M21θ1 +M22θ2 + c112 θ21 + g2 = τ2

or

[m1a2C1 +m2(a21 + a2C2 + 2a1aC2C2) + I1 + I2]θ1 + [m2(a2C2 + a1aC2C2) + I2]θ2

−m2a1aC2S2θ22 − 2m2a1aC2S2θ1θ2

+(m1aC1 +m2a1)gC1 +m2gaC2C12 = τ1

[m2(a2C2 + a1aC2C2) + I2]θ1 + [m2a

2C2 + I2]θ2

+m2a1aC2S2θ21

+m2gaC2C12 = τ2



Dynamic model


Properties of the Euler-Lagrangian dynamic model

The Euler-Lagrange dynamic model is characterized by some structuralproperties, concerning in particular:

1 The inertia matrix M(q);

2 The vector c(q, q) = C(q, q)q;

3 The vectors g(q) and D q;

4 Linearity with respect to the geometric/mechanical parameters.

It is necessary the definition of proper vector norms in IRn

1-norm: ||x||1 =n∑

i=1

|xi |

2-norm: ||x||2 =

(

n∑

i=1

x2i

)1/2

(Euclidean norm)

p-norm: ||x||p =

(

n∑

i=1

|xi |p

)1/p

∞-norm: ||x||∞ = max1≤i≤n

|xi |



Dynamic model



1. Properties of matrix M(q)

1 M(q) ∈ IRn×n is symmetric, positive definite and depends on the manipulator

configuration q

2 M(q) is upper and lower bounded:

µ1I ≤ M(q) ≤ µ2I

that is

xT (M(q) − µ1I)x ≥ 0 xT (µ2I−M(q))x ≥ 0

3 also M−1(q) is bounded1

µ2I ≤ M−1(q) ≤

1

µ1I

4 in case of revolute joints, then µ1, µ2 are constant (not function of q) since theelements of M(q) are functions of sin(qi ) or cos(qi )

5 in case of prismatic joints, µ1, µ2 may result scalar functions of q

6 since M(q) is bounded, then

M1 ≤ ||M(q)|| ≤ M2

for some properly defined norm (1, 2, p,∞)



Dynamic model



2. Properties of vector c(q, q) = C(q, q)q1 C(q, q)q is a quadratic function of q2 the generic k-th element of vector c(q, q) = C(q, q)q can also be witten as

ck(q, q) = qTSk(q)q

Sk(q) =1

2

(

∂mk

∂q+

(∂mk

∂q

)T

−∂M

∂qk

)

mk = k-th col. of M

3 it results that||C(q, q)q|| ≤ vb ||q||

2

4 in case of rotative joints, then vb is constant (not function of q) since wehave only transcendental functions (sin(qi ) or cos(qi ))

5 in case of prismatic joints, then vb may result a scalar function of q6 for any choice of C(q, q), then matrix N(q, q) = M(q, q)− 2C(q, q)

verifies:qTN(q, q)q = 0

7 with a proper choice of the elements of C(q, q) (Christoffel symbols),

matrix N(q, q) = M(q, q)− 2C(q, q) is skew-symmetric, i.e.

xTN(q, q)x = 0 ∀x



Dynamic model



3. Properties of vectors g(q) and Dq

1 the friction term Dq is bounded:

||Dq|| ≤ dmax ||q||

2 the gravity term g(q) is bounded

||g(q)|| ≤ gb(q)

3 in case of revolute joints, gb is constant (does not depend on q) since qi dependson sinusoidal functions (sin(qi ) or cos(qi ))

4 in case of prismatic joints, then gb may result function of q.



Dynamic model



4. Linearity properties (in the geometrical/mechanical parameters)

The dynamic model of a manipulator:

in general is a non linear function of q, q, q and with dynamic couplingeffects among the joints,

is a linear function of the geometrical/mechanical parameters of the links(i.e. masses, inertia, friction, ...)

M(q)q+ C(q, q)q+Dq+ g(q) = τ

Y(q, q, q) α = τ



Dynamic model


Dynamic modelEuler–Lagrange

Some additional considerations

In deriving the dynamic model, the actuation system has not been taken into account.This is normally composed by:

motors

reduction gears

trasmission system.

The actuation system has several effects on the dynamics:

if motors are installed on the links, then masses and inertia are changed

it introduces its own dynamics (electromechanical, pneumatic, hydraulic, ...) thatmay be non negligible (e.g. in case of lightweight manipulators)

it introduces additional nonlinear effects such as backslash, friction, elasticity, ...

Notice that these effects could be considered by introducing suitable terms in thedynamic model derived on the basis of the Euler-Lagrangian formulation.



Dynamic model


Dynamic modelNewton–Euler

Based on the balance of forces and torques acting on each link. A recursiveformulation is obtained: forward recursion for the computation of the “propagation”of accelerations and velocities, backward recursion for the computation of the“propagation” of forces and torques. The basic equations are:

1 Newton’s second law of motion: (conservation of momentum mv)

f =d(m v)

dt

where m is the mass, v the linear velocity, f the sum of all the applied externalforces and mv the momentum. Since in robotics the masses of the links areconstant, we have the well known Newton equation f = m a

2 Euler’s law of motion (conservation of the angular momentum L = Iω):

n =d(

0I 0ω)

dt

where 0I is the moment of inertia (rotational inertia) expressed in an inertialframe FF0 placed in the center of mass, 0ω the rotational velocity, n the sum ofthe torques applied to the rigid body.



Dynamic model


Newton-Euler model

Let us consider the generic i-thlink of the manipulator

Define, in an inertial frame, the following parameters (CoM = center of mass):

mi mass of the linkIi rotational inertia (inertia tensor)ri−1,Ci

vector from FFi-1 to the CoM Ci

ri,Ci vector from FFi to the CoM Ci

ri−1,i vector from FFi-1 to FFipCi linear velocity of Ci

pi linear velocity of FFiωi rotational velocity of the link

pCi acceleration of Ci

ωi acceleration of Cig gravity accelerationfi force applied on Li by Li−1−fi+1 force applied on Li by Li+1νi torque applied on Li by Li−1 wrt FFi-1−νi+1 torque applied on Li by Li+1 wrt FFi



Dynamic model


Newton-Euler model

Therefore, for each link we have:

Newton’ law: fi − fi+1 +mig = mi pCi

Euler’s law: νi + fi × ri−1,Ci − νi+1 − fi+1 × ri,Ci =ddt(Iiωi)

Notice that the gravitational force mig does not generate any torque, since it isconcentrated at the centre of mass.

For the computation of the angular momentum, it is preferable to expressmatrix Ii in FFi (that is i Ii ), where it assumes a constant value, rather than inFF0. As a matter of fact, we know that

Ii = Rii IiR

Ti

being Ri the rotation matrix between FFi and FF0. It is possible to proof thatthe Euler law results:

νi + fi × ri−1,Ci − νi+1 − fi+1 × ri,Ci = Ii ωi + ωi × (Ii ωi)



Dynamic model


Newton-Euler model

Once the forces/torques applied on the links are known by the Newton/Eulerlaws, the generalized force τi applied on the i-th joint by the link Li−i can becomputed as the projection along the zi−1 axis of fi or νi .

We have:

τi =

{νTi zi−1 rotational joint

fTi zi−1 linear joint



Dynamic model


Newton-Euler model

The Newton-Euler laws

Newton’ law: fi − fi+1 +mig = mi pCi

Euler’s law: νi + fi × ri−1,Ci − νi+1 − fi+1 × ri,Ci =ddt(Iiωi)

=⇒ νi + fi × ri−1,Ci − νi+1 − fi+1 × ri,Ci = Ii ωi + ωi × (Ii ωi )

require the computation of the linear (pCi ) and rotational (ωi ) acceleration oflink Li .

These accelerations may be obtained on the basis of the correspondingvelocities. We have:

pi =

{pi−1 + ωi × ri−1,i rotational joint

pi−1 + zi−1d + ωi × ri−1,i linear joint

ωi =

{ωi−1 + zi−1θi rotational joint

ωi−1 linear joint



Dynamic model


Newton-Euler model

By derivation, we obtain the expression of the accelerations

pi =

{

pi−1 + ωi × ri−1,i + ωi × (ωi × ri−1,i ) rot. jointpi−1 + zi−1d + 2ωi × zi−1d + ωi × ri−1,i + ωi × (ωi × ri−1,i ) linear joint

ωi =

{

ωi−1 + zi−1θi + ωi−1 × zi−1θi rotational jointωi−1 linear joint

where, in the computation of the linear acceleration for a linear joint, wetake into account that ri−1,i = zi−1d + ωi−1 × ri−1,i , and that ωi = ωi−1.

the CoM acceleration pC1 can be computed from pi , since pCi = pi + ri,Ci ;by deriving twice, we have

pCi = pi + ωi × ri,Ci + ωi × (ωi × ri,Ci )



Dynamic model


Dynamic modelNewton–Euler

The Newton-Euler approach does not give a mathematical model in closed form,it is rather a recursive algorithm: the motion of each link is affected by themotion of the other links through the velocity/acceleration kinematic coupling.Once the joints positions, velocities and accelerations are known, it is possible tocompute the corresponding variables for the links

In order to (recursively) compute the velocities and accelerations, we start fromthe base frame (where we have known border conditions: v = a = 0) and theniteratively “move” to the distal link

Then, given the (possible) external forces/torques applied to the end-effector(measured e.g. with a proper sensor), with the Newton-Euler laws it is possibleto compute (backward) the forces/torques applied to the links.

A three-step recursive algorithm is then obtained:

1 forward recursion to propagate velocity and acceleration2 backward recursion to propagate forces and torques3 computation of the generalized forces applied to the joints



Dynamic model


Newton-Euler model: algorithm

1 once the proximal boundary conditions ω0, p0 − g0, ω0 are known(angular velocity, CoM linear acceleration and angular acceleration of linkL0), the angular velocities, CoM linear accelerations and angularaccelerations ωi , ωi , pi , pCi are computed, for links Li , i = 1, . . . , n

2 once the distal boundary conditions fn+1, νn+1 (force and torque applied tothe manipulatro end-effector) are known, the forces and torques fi , νiapplied at each link Li are computed

3 the generalized forces τi , i = 1, . . . , n, are computed as

τi =

{νTi zi−1 (+Fv,i θi + fs,i ) rotational joint

fTi zi−1 (+Fv,i di + fs,i ) linear joint

where Fv,i , fs,i are viscous and static friction coefficient possibly presentat the joint



Dynamic model


Example

Dynamic model of a 2 dof planar manipulator

Border conditions for velocities, accelerations, forces and torques:

p0 = [0, −g , 0]T ω0 = ω0 = 0

f3 = 0 ν3 = 0



Dynamic model


Example

By defining the variables in the links’ frames, we have for the first link

r1,C1 = [ aC1 − a1, 0, 0 ]T r0,1 = [ a1, 0, 0 ]T

while for the second link

r2,C2 = [ aC2 − a2, 0, 0 ]T r1,2 = [ a2, 0, 0 ]T

The rotation matrices are

0R1 =

[C1 −S1 0S1 C1 00 0 1

]

1R2 =

[C2 −S2 0S2 C2 00 0 1

]

2R3 =

[1 0 00 1 00 0 1

]



Dynamic model


Example

The recursive procedure is as follows:

Forward recursion

Link 1:

ω1 = 1R0(ω0 + z0θ1) =

[00θ1

]

ω1 = 1R0(ω0 + z0θ1 + ω0 × z0θ1) =

[00θ1

]

p1 = 1R0p0 + ω1 × r0,1 + ω1 × (ω1 × r0,1) =

−gS1 − a1θ

21

−gC1 + a1θ10

pC1 = p1 + ω1 × r1,C1 + ω1 × (ω1 × r1,C1) =

−gS1 − aC1θ

21

−gC1 + aC1 θ10



Dynamic model


Example

Link 2:

ω2 = 2R1(ω1 + z1θ2) =

[

00

θ1 + θ2

]

ω2 = 2R1(ω1 + z1θ2 + ω1 × z1θ2) =

[

00

θ1 + θ2

]

p2 = 2R1p1 + ω2 × r1,2 + ω2 × (ω2 × r1,2) =

−gS12 − a1C2 θ21 + a1S2θ1 − a2(θ1 + θ2)

2

−gC12 + a1S2θ21 + a1C2 θ1 + a2(θ1 + θ2)

0

pC2 = p2 + ω2 × r2,C2 + ω2 × (ω2 × r2,C2 ) =

−gS12 − a1C2 θ21 + a1S2θ1 − aC2 (θ1 + θ2)

2

−gC12 + a1S2θ21 + a1C2 θ1 + aC2 (θ1 + θ2)

0



Dynamic model


Example

Backward recursion

Link 2:

f2 = 2R3f3 +m2pC2= m2pC2

ν2 = 2R3ν3 − f2 × (r1,2 + r2,C2) + 2R3f3 × r2,C2

+ 2I2ω2 + ω2 × (2I2ω2)

=

00

m2aC2gC12 +m2a1aC2

(C2θ1 + S2θ21) + (I2 +m2a2C2)(θ1 + θ2)

τ2 = νT2 z1 = (I2 +m2a2C2)θ2 +m2(I2 + a2C2

+ a1aC2C2)θ1 +m2a1aC2

S2θ21 +m2aC2

gC12



Dynamic model


Example

Link 1:f1 =

1R2f2 + m1pC1

=

(m1 + m2)gS1 − m2a1θ21 − m2aC2C2(θ1 + θ2)

2 − m2aC2S2(θ1 + θ2) − m1aC1θ21

(m1 + m2)gC1 + m2a1θ1 − m2aC2S2(θ1 + θ2)2 − m2aC2C2(θ1 + θ2) + m1aC1θ1

0

ν1 = 1R2ν2 − f1 × (r0,1 + r1,C1 ) +1R2f2 × r1,C1 + 1I1ω1 + ω1 × (1I1ω1)

=

00

m1aC2gC12 + m2a1aC2 (C2 θ1 + S2 θ21) + (I2 + m2a

2C2

)(θ1 + θ2) + m2a1gC1+

+m2a21 θ1 − m2aC2S2(θ1 + θ2)

2 + m2a1aC2C2(θ1 + θ2) + m1aC1gC1 + m1a2C1

+ I1θ1

τ1 = νT1 z0

= (I1 + I2 + m1a2C1

+ m2(a21 + a2C2

+ 2a1aC2C2))θ1 + (I2 + m2(a2C2

+ a1aC2C2))θ2 +

−m2a1aC2S2(2θ1θ2 + θ22) + (m1aC1 + m2a1)gC1 + m2aC2gC12



Dynamic model


Dynamic modelFinal considerations

Both the Euler-Lagrange and Newton-Euler methods allow to define the dynamics ofan industrial manipulator. From the point of view of the final result, the two methodsare equivalent.

Euler-Lagrange formulation

it considers at the same time all the links of the manipulator and is based on theLagrange function L = K − P (difference between the kinetic and potentialenergy)

it is easier to be understood, and offers a more systematic approach

it gives the motion equations in closed form (useful for designing control laws)

it is easier to consider complex mechanical effects, such as elastic deformation ofthe links

Newton-Euler formulation

intrinsically, it is a recursive formulation, where each link is considered separatelyand recursively, evaluating the reciprocal couplings among links in terms ofvelocities, accelerations and forces

it gives better performances, from the computational point of view, for theevaluation of the forces required to obtain a desired motion (trajectory)

being a recursive formulation, it is more suitable to implement computationalalgorithms (simulation), although -with a proper formulation- also theLagrangian approach is quite performing.



Dynamic model



We have seen that also for the dynamics we have:

Direct Dynamic Problem: computation of the time evolution of q(t) (and then ofq(t), q(t)), given the generalized force vector (torques and/or forces) τ(t)applied at joints, the possible external forces applied at the end-effector, and theinitial conditions q(t = t0), q(t = t0)

Inverse Dynamic Problem: computation of the generalized force vector τ(t)necessary to obtain a desired motion (trajectory) q(t), q(t), q(t), given thepossible forces applied at the end-effector

The solution of the direct problem is relevant for simulations, when it is desired tocompute the motion of the manipulator as a consequence of the application of a givenset of torques at joints and forces at the end-effector. Once the acceleration is known,velocities and positions can be computed by integration of a set of nonlineardifferential equations.

With the Lagrangian formulation, we have

M(q)q + C(q, q)q + Dq + g(q) = τ + JT(q)Fa

Then, once the state of the system q(tk ), q(tk ), is known at t = tk , the accelerationq(tk ) may be computed as

q = M−1(q)[

τ + JT(q)Fa − C(q, q)q − Dq − g(q)]

and then, by numerical integration, the new state variables q(tk+1), q(tk+1), with

tk+1 = tk +∆t.



Dynamic model



It may be of interest to obtain the dynamic model in the work-space andnot in the joint-space. For the sake of simplicity, let us consider a 6 dofmanipulator, not in a singular configuration

Since M(q)q + C(q, q)q + Dq + g(q) = τ + JT(q)Fa, then

q = M−1(q)[

τ + JT(q)Fa − C(q, q)q − Dq − g(q)]

Moreover, by deriving x = J(q)q with respect to time, we have:

x = J(q)q + J(q)q

Therefore

M(x)x + C(x , x)x + Dx + g(x) = F + Fa, τ = JTF

with:

M = (JM−1JT )−1 = J−TMJ−1

C = M(JM−1CJ−1 − JJ−1) = J−TCJ−1 − MJJ−1

g = MJM−1g = J−T g


Documents

Elements of Kinematics and Dynamics of Industrial Robotscampus.unibo.it/218969/1/IRM_01_Kinematics_Dynamics.pdf · robot executes its tasks. It is the set of all the points (conﬁgurations)