control of industrial robots robot dynamics
TRANSCRIPT
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๐ก๐ก ๐ฑ๐ฑ = 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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๐ฟ๐ฟ ๐ช๐ช, ๏ฟฝฬ๏ฟฝ๐ช = ๐๐ ๐ช๐ช, ๏ฟฝฬ๏ฟฝ๐ช โ ๐๐ ๐ช๐ช
๐๐๐๐ = ๏ฟฝ๐๐=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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
ฯ๐๐๐๐ = ๐๐๐๐๐๐ฯ๐๐
(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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๐ฃ๐ฃ๐๐๐๐๐๐๐๐
๐ฃ๐ฃ๐๐๐๐๐๐๐๐
=
๐ณ๐ณ๐๐โ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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
๐๐
๏ฟฝ๐๐=1
๐๐๐๐๐๐๐๐๐๐ ๐ช๐ช๐๐๐๐๐๐
๏ฟฝฬ๏ฟฝ๐๐๐ ๏ฟฝฬ๏ฟฝ๐๐๐
๐๐๐๐๐๐๐๐๐๐
=12๏ฟฝ๐๐=1
๐๐
๏ฟฝ๐๐=1
๐๐๐๐๐๐๐๐๐๐ ๐ช๐ช๐๐๐๐๐๐
๏ฟฝฬ๏ฟฝ๐๐๐๏ฟฝฬ๏ฟฝ๐๐๐
Equations of motion
Control of industrial robots โ Robot dynamics โ Paolo Rocco
From Lagrangeโs equations we obtain:
where:
๏ฟฝ๐๐=1
๐๐
๐๐๐๐๐๐ ๐ช๐ช ๏ฟฝฬ๏ฟฝ๐๐๐ + ๏ฟฝ๐๐=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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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๐๐
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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 = ๐ฃ๐ฃ๐๐1
๐๐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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Two-link Cartesian manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Two-link Cartesian manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
Link 1
๐๐๐๐๐๐1 = ๐ฃ๐ฃ๐๐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 = ๐ฃ๐ฃ๐๐1
๐๐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
Two-link planar manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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!
Two-link planar manipulator
๐๐11 = ๐๐1๐12 + ๐ผ๐ผ1 + ๐๐2 ๐๐12 + ๐22 + 2๐๐1๐2๐๐2 + ๐ผ๐ผ2๐๐12 = ๐๐21 = ๐๐2 ๐22 + ๐๐1๐2๐๐2 + ๐ผ๐ผ2๐๐22 = ๐๐2๐22 + ๐ผ๐ผ2
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Two-link planar manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Two-link planar manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๐๐๐๐ : 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:
Two-link planar manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Two-link planar manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๐ฆ๐ฆ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.
Two-link planar manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๏ฟฝฬ๏ฟฝ๐ฉ๐ถ๐ถ๐๐ 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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Newton-Euler formulation
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Two-link planar manipulator
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
ฯ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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
ฯ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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๐๐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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๐๐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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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)
Newton-Euler formulation
it is a computationally efficient recursive method
Euler-Lagrange vs. Newton-Euler
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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โฆ.
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
๐๐ ๐ช๐ช ๏ฟฝฬ๏ฟฝ๐ช + ๐ง๐ง ๐ช๐ช, ๏ฟฝฬ๏ฟฝ๐ช = ฯ
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
Control of industrial robots โ Robot dynamics โ Paolo Rocco
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