Script Dynamics
Script Dynamics
Dynamics
3.1 Introduction
In most robotic applications the goal is to find a multi-body dynamics description for-
mulated as
M (q) q̈ + b (q, q̇) + g (q) = τ + Jc (q)T Fc (3.1)
consisting of the following components:
59
generalized coordinates which adhere to certain minimization principles, thus resulting
in trajectories which automatically satisfy the kinematic constraints of the system. The
third and arguably most useful approach of all is presented in section 3.5 and is that
of the Projected Newton-Euler method, which manages to combine the advantages of
both the Newton-Euler and Lagrange methods, thus constituting a reformulation of the
Newton-Euler method in terms of the generalized coordinates and hence directly con-
siders the feasible motions of the system, i.e., those satisfying the applicable kinematic
constraints.
Although the methods are often presented to be quite different, and may appear as
such initially, their equivalence will become quite clear by the end. In the end, it is up
to the reader to identify the appropriate approach to use for modeling a given robotic
system or problem setting.
dm
r̈
dF
r
The most basic formulation of Newtonian mechanics describes the motion of point-
masses, i.e., particles with mass m and of infinitely small dimensions, were the entire
mass is concentrated at the single point defined by the position vector r. Keep in mind
that a point-mass does not have an orientation as it is impossible to define any geometry.
This means that we need only consider the Newton’s second law to fully describe the
motion of the system:
r̈m = F (3.2)
Considering again the fact that we have defined the mass to be of infinitely small di-
mensions, we can think of it as an infinitesimal mass dm subject to infinitesimal forces
dF concentrated at the position of the particle:
r̈dm = dF (3.3)
As we will see in the sections that follow, these relations still play an important role in
the derived methods. (3.2) still applies for the case of computing the linear dynamics
of the Center of Mass (CoM) of rigid bodies, while (3.3) is used for to describe the
effects of internal forces in a system via the Principle of Virtual Work.
60
3.2.2 Virtual Displacements
We will now introduce the concept of variational notation via the δ operator. Essen-
tially, this operator behaves exactly as the differential d operator except that it has a
completely different meaning. The differential is defined to describe an infinitesimal
quantity which is then used to define “rates of change” of one quantity with respect
to another. On the contrary the variation of a quantity describes, for a fixed instant in
time, all possible directions the quantity may move in while adhering to the applicable
constraints.
The most important fact to remember regarding a variation, is that if it taken with
respect to quantity which is a function of time, then any temporal dependence is disre-
garded completely. To demonstrate this property, we will define the admissible varia-
tion of a position vector, called a virtual displacement δr, and assume that the position
is a function of generalized coordinates q and time t, thus r = r(q, t). By applying the
chain rule over the nq elements of q we compute the variation δr:
nq
X ∂r
δr(q, t) = δqk (3.4)
∂qk
k=1
∂r
Notice that the expression in (3.4) does not include ∂t , and can thus be understood to
be a purely geometric interpretation.
Ψ = Ω̇
δΦ Ω = ωIB
dm
{B}
dF
ρ
r vS
S
aS
A body with mass existing in 3D Cartesian space, as shown in Fig. 3.2, is very
simply a large number of particles closely placed together to form a single rigid body.
Each infinitesimal point-mass dm is subject to the motion of the total body and hence
can be assigned an absolute position and velocity at any instant in time. Lets now
consider another point S on the body, such that we can define the relative position ρ of
dm w.r.t. the point S.
61
Thus one can then use the rigid body kinematics formulation introduced in sec-
tion 2.7 to describe the motion an arbitrary point-mass dm defined on the body B via
another body point S:
r = rOS + ρ (3.5)
vs
ṙ = vS + Ω × ρ = I3×3 − [ρ]× (3.6)
Ω
as
r̈ = aS + Ψ × ρ + Ω × (Ω × ρ) = I3×3 − [ρ]× + [Ω]× [Ω]× ρ (3.7)
Ψ
Where, rOS is the absolute position of body point S, ρ is the relative position of dm
w.r.t S, and vS , Ω, aS , and Ψ are the absolute velocities and accelerations of point S.
Applying (3.4) to (3.5), we get the expression for the virtual displacement of the body
element dm:
δrs
δr = δrS + δΦ × ρ = I3×3 − [ρ]× (3.8)
δΦ
and δΦ is the variation of the infinitesimal rotation of the local body-fixed frame
defined at S and describes all constraint-compatible changes in orientation of the local
frame at S. Note the bold face over the entire quantity of δΦ; this is due to the fact that
the variation is not taken w.r.t. an orientation quantity, just like angular velocity is not
the result of time-differentiating an orientation.
62
We can extend this further by considering again an infinitesimal dm in a body B. By
also considering d’Alambert’s Principle describing dynamic equilibrium for particles,
the virtual work integrated over the entire geometry of body B becomes:
Z
δW = δrT · (r̈dm − dFext ) = 0, ∀ δr (3.13)
B
Thus the virtual work is zero for all displacements δr, those changes in configura-
tion which are consistent with the constraints of the system. Although this formulation
might seems somewhat complicated, it will become clearer in the continuation that it
describes most of the well know concepts in a quite compact and well understandable
manner.
we get
T
δrs I3×3 m 0 as 0 Fext δrs
0 = δW + − ∀ .
δΦ 0 ΘS Ω [Ω]× ΘS Ω Text δΦ
(3.18)
63
In order to define the laws of conservation of linear and angular momentum we intro-
duce the definitions:
A free moving body needs to fulfill that the change in linear momentum equals the sum
of all external forces
T
δrs ṗS Fext δrs
0 = δW = − ∀ , (3.23)
δΦ ṄS Text δΦ
where Fext,S are the resultant external forces that act through the COG and Text are the
resultant external torques. External forces which do not act through the COG need to
be shifted to an equivalent force/moment pair of which the force acts through the COG.
Please note again that for numerical calculation, the terms of the change in linear and
angular momentum must be expressed in the same coordinate system. For the inertia
tensor Θ we must apply B Θ = CBA · A Θ · CTBA .
64
Fi2
Ψi Ωi
aSi
vSi
Fi1
Fg
rOSi
Figure 3.3: Free-cut of multi-body system with constraint forces due to joints
L=T −U (3.26)
65
generalized forces τ :
d ∂L ∂L
− =τ (3.27)
dt ∂ q̇ ∂q
In the most general case, the Lagrangian is a function of the generalized coordinates
and velocities q and q̇, and it may also have an explicit dependence on time t, hence
we redefine the aforementioned scalar energy functions as T = T (t, q, q̇) and U =
U(t, q), thus L = L(t, q, q̇).
In the end, one of the most notable properties of this formulation is the capacity to
eliminate all internal reaction forces of the system from the final EoM, in contrast to
the Newton-Euler formulation where there they are explicitly accounted for. To apply
this method to derive EoM of a complex multi-body system there are additional aspects
which must be considered before one can applying the three aforementioned concepts.
These are presented in a concise overview at the end of this section, and are explained
in the immediate continuation.
For every body Bi in the system, although the linear part may be computed while
expressed in some frame A, it may be more convenient to compute the rotational kinetic
energy using expressions in another frame B, rotated w.r.t to A, where the inertia matrix
ΘS,i may have a diagonal form, i.e. the basis vectors of B are principle w.r.t. the
mass distribution. This computation will yield correct results as long as both linear
and angular velocities A ṙS,i and B ΩS,i express the absolute velocities of the body, i.e
velocities w.r.t. to the inertial frame.
We now need to express the kinetic energy as a function of the generalized quanti-
ties. To achieve this, we make use of the Jacobian matrices described by (2.163) and
(2.164), but computed for each body Bi instead of the end-effector. This then allows
us to use the following kinematic relationships:
Replacing these relationships into the definition of the kinetic energy in (3.28), results
in the kinetic energy expressed in the generalized coordinates:
nb
!
1 T X
T (q, q̇) = q̇ JTSi mJSi + JTRi ΘSi JRi q̇ (3.31)
2 i=1
| {z }
M(q)
The underlined quantity M(q) is defined as the generalized mass matrix or generalized
inertia matrix, and as we will see in the continuation, is solely responsible for gener-
ating both the inertial and non-linear centrifugal and Coriolis force terms in the final
EoM.
66
3.4.3 Potential Energy
In typical mechanics problems, there are two basic contributions to the potential energy
in a system; masses contributing via their gravitational potential energy, and elastic
elements via the energy stored when deflected from rest.
In the first case, each body Bi holds potential energy due to the effect of the gravita-
tional potential field of the earth. Although not linear on large scales, in most cases we
approximate its effect on bodies via a uniform and unidirectional potential field defined
along the unit vector I eg acting through the CoM of each body. Knowing the position
rSi of the CoM of each body This then allows us to compute the potential energy of
each body as:
Fgi = mi g I eg (3.32)
nb
X
Ug = − rTSi Fgi (3.33)
i=1
Note that the zero energy level can be arbitrarily chosen. In addition to gravitational po-
tential energy contributions, many applications involve elastic elements such as springs
or other compliant components. If such a element Ej can be reasonably approximated
to have a linear deflection-to-force or deflection-to-torque relationship, then the poten-
tial energy contribution can be described by the following relation:
1 2
U Ej = kj (d(q) − d0 ) (3.34)
2
Where d(q) expresses the instantaneous configuration (i.e. deflection) of the elastic
element as a function of generalized coordinates. d0 is defined as the rest configuration
where no forces are exerted by the element, e.g., the unsprung length of a linear spring.
Note: Spring forces may equivalently be considered as external forces whose mag-
nitude and possibly direction depends on the generalized coordinates q. The formalism
is similar to introducing actuator torques.
67
combinations of the general velocities:
nq
X
ak,j (q) q̇k + a0,j (t), j = 1, 2..nc,m (3.35)
k=1
For nc,m motion constraints, we can stack all ak,j (q) coefficients to a motion constraint
Jacobian matrix Jm :
a1,1 . . . a1,nc,m
Jm = ... ...
..
. (3.36)
anq ,1 ... anq ,nc,m
On the other hand, there are situations where constraints will be defined as functions
of the configuration, i.e. as scalar functions fj (q) : Rnq → R, and are known as
configuration constraints. In these cases, computing the gradient of such a function
w.r.t the generalized coordinates results in the constraint Jacobian matrix described in
section 2.10.4.
In total, incorporating these constraints results in a description of the constrained
EoM, since the additional constraints being applied for by the parameters λ are not
account for in the definition of q. Thus the final EoM, are derived using the so-called
Constrained Euler-Lagrange (CEL) equation, also known as the Euler-Lagrange of the
first kind:
T
d ∂L ∂L T ∂fc
− + Jm λ m + λc = τ (3.37)
dt ∂ q̇ ∂q ∂q
Where λm ∈ Rnc,m is the vector of Lagrangian multipliers for the motion con-
straints, while λc ∈ Rnc,c is the vector of Lagrangian multipliers for the configuration
constraints.
In conclusion, we note that it should be clear why this method is also referred to
as being energy-based, as the equations of motion are derived from a scalar energy
function, and thus, we did not make explicit use of any Cartesian vector quantities. All
the kinematic information is encoded into the definition of the generalized coordinates,
which are then used to define the scalar Lagrangian, and then applied to (3.27) in order
to produce the EoM. This is in stark contrast to Newton-Euler methods which explicitly
deal with Cartesian vector quantities.
68
3.5.2 Deriving Generalized Equations of Motion
As outlined in section 3.2.4, the use of generalized coordinates allows to describe the
motion and virtual displacements of bodies in ways which are consistent with the ap-
plicable constraints. Thus, we can rewrite principles of linear and angular momentum
applied to each body Bi as:
ṗSi maSi
=
ṄSi ΘSi ΨSi + ΩSi × ΘSi ΩSi
(3.39)
mJSi mJ̇Si q̇
= q̈ +
ΘSi JRi ΘSi J̇Ri q̇ + JRi q̇ × ΘSi JRi q̇
Furthermore, using the expressions for virtual displacements of generalized coordi-
nates, we can rewrite the principle of virtual work as follows:
Xnb T
δrSi ṗSi Fext,i δrs
0 = δW = − ∀ , (3.40)
δΦSi ṄSi Text,i δΦSi consistent
i=1
nb
X
T ṗSi Fext,i
= δq − ∀δq (3.41)
ṄSi Text,i
i=1
69
3.5.3 External Forces & Actuation
As we have seen in section 3.4.4, in order to account for external forces in EoM ex-
pressed in generalized coordinates, we can project Cartesian forces and torques onto
the space of generalized coordinates using appropriate Jacobian matrices. The latter
project the effect of the forces onto the subspace of the generalized coordinates on
which work is done. We will now consider how to effectively account for all acting
forces and torques on a multi-body system.
Lets assume we know that nf,ext external forces Fj , j ∈ {1, . . . , nf,ext } and
nm,ext external torques Tk , k ∈ {1, . . . , nm,ext } act on the system (on any body). The
generalized forces τ F,ext due to the external forces can be calculated in the following
way: Assume the Cartesian force Fj acts on point Pj and the translational Jacobian of
that point is JP,j,trans . Then the generalized forces are computed as
nf,ext
X
τ F,ext = JP,j T Fj . (3.49)
j=1
Similarly, the generalized forces τ T,ext can be evaluted by projecting each Cartesian
torque by the rotational Jacobian of the body they act on, which we shall call JB,k,rot :
nm,ext
X
τ T,ext = JR,k T Text,k . (3.50)
k=1
Finally, the contributions of external forces and torques can simply be added as they
are both represented in the space of generalized coordinates:
For the special case of actuator forces or torques that act between two body links,
we need only consider the kinematic relations defined between those two (successive)
body links. Thus, an actuator acting between body links Bk−1 and Bk , imposes a
force Fa,k and/or torque Ta,k on both equally and in opposite directions, i.e., action-
reaction. What thus need to compute are the Jacobian matrices to appropriately project
onto the dimensions of q.
To this end, we recall that Jacobian matrices, when expressed in the same refer-
ence frame, can be simply added or subtracted, and thus having computed JSk−1 , JSk ,
JRk−1 , and JRk from our previous analysis of the kinematics, we can compute the
contribution of the actuators to the generalized forces as:
T T
τ a,k = JSk − JSk−1 Fa,k + JRk − JRk−1 Ta,k (3.52)
In the most simple, and in fact also the most common, case of an actuator acting in
the direction of a generalized coordinate qj , the difference between the Jacobian at the
point of action and reaction multiplied with the force or torque provides the single entry
in the generalized force vector τj . Thus, the vector of total external generalized forces
is simply a combination of the aforementioned parts, accounting for nA joint actuators
and nB body links:
nA
X
τ = τ a,k + τ ext (3.53)
k=1
Comments:
70
• The translational Jacobian of an arbitrary point on a body can be obtained by
starting with the translational Jacobian of the COM of that body and applying
(2.154).
• Alternatively, one may transform all forces that do not act through the COM of
a body into an equivalent force/moment pair which allows usage of the COM
translational and rotational Jacobians.
• Actuator forces/torques are no different to any other external forces/torques ex-
cept that both the torque/force and its reaction act on the system.
71