Universit`
a di Pisa
Dottorato di Ricerca in Ingegneria Industriale
Curriculum in Ingegneria Meccanica Ciclo XXX
Modeling and optimal control of multibody systems with
intermittent contacts
Author
Silvia Manara Supervisors
Prof. Marco Gabiccini Ing. Alessio Artoni
Prof. Gabriele Pannocchia
Coordinator of the PhD Program Prof. Giovanni Mengali
A chi ha avuto la pazienza di ascoltare
e la cura di accompagnare.
Abstract
This thesis focuses on the application of numerical optimal control methods to the trajectory planning of multibody systems (MBS). The trajectory planning prob-lem consists in determining a suitable sequence of control actions to be taken in order for a dynamic system to accomplish a specified task. The difficulties in planning trajectories for MBS are mainly due to their complex, nonlinear, fast dynamics. Moreover, in contexts such as robotic locomotion or manipulation, in which the system necessarily has to interact with the surrounding environment through intermittent contact forces, additional issues arise, due to the discon-tinuities caused by impacts and friction. Numerical optimal control offers the possibility to handle significantly complex models and, most notably, to exploit the richness of their dynamics in the accomplishment of the prescribed task.
The thesis aims to investigate the modeling of the aforementioned complex systems, as well as their interactions with the environment, in order for the re-sulting model to be suitably dealt with by numerical optimal control techniques. The application of such methods to several benchmark systems is presented, that share the feature of being not easily — or at least not effectively — controllable by means other than numerical.
Finally, the thesis addresses issues related to the presence of uncertainties that might compromise the applicability of the planned control trajectories to a real system. Rather than traditional control strategies based on feedback, alternative strategies are investigated that rely on a stochastic model of the disturbance in order to include considerations on its effects in the trajectory optimization prob-lem. Such strategies allow to plan trajectories that provide certain guarantees of stability and robustness in the presence of uncertainties. Specifically, the appli-cability of an already existing framework for robust optimal control of periodic nonlinear system in general was extended to MBS with intermittent contacts.
Questa tesi verte sull’applicazione di metodi di controllo ottimo numerico alla pianificazione di traiettorie per sistemi multibody. Il problema di pianificazione di traiettoria consiste nel determinare una sequenza temporale di azioni di controllo tali da permettere a un dato sistema dinamico di compiere una specifica opera-zione. Le difficolt`a nella pianificazione di traiettorie per sistemi multibody sono principalmente dovute alla loro dinamica veloce, complessa e nonlineare. Inoltre, in contesti quali la locomozione o la manipolazione robotica, in cui il sistema de-ve necessariamente interagire con l’ambiente esterno attrade-verso forze di contatto intermittenti, insorgono ulteriori problematiche dovute alle discontinuit`a causate da fenomeni di impatto e di attrito. Il controllo ottimo numerico offre la possibi-lit`a di trattare modelli significativamente complessi e, soprattutto, di sfruttarne la ricchezza dinamica nell’adempimento del compito prescritto.
Al fine di indagare la modellazione dei suddetti sistemi dinamici, nonch´e delle loro interazioni con l’ambiente, in modo tale che il modello risultante sia adatto ad essere trattato con tecniche di controllo ottimo numerico, nella tesi vengono pre-sentati alcuni casi di studio che dimostrano l’efficacia di questi metodi nel trattare diversi sistemi, tutti accomunati dal fatto di non essere facilmente — o, quanto meno, efficientemente — controllabili con tecniche che non siano numeriche.
Infine, la tesi affronta anche problematiche relative alla presenza di incertezze che possano compromettere l’applicabilit`a delle traiettorie di controllo pianificate al sistema reale. Piuttosto che tecniche di controllo tradizionali basate sulla retroazione, vengono presi in esame metodi alternativi, che si avvalgono di un modello stocastico del disturbo al fine di considerarne gli effetti nel problema di pianificazione della traiettoria. Questi metodi permettono cos`ı di pianificare traiettorie che offrano determinate garanzie di stabilit`a e robustezza in presenza di incertezze. In particolare, l’applicabilit`a di un framework per il controllo ottimo robusto di generici sistemi periodici nonlineari `e stata estesa alla pianificazione di traiettorie periodiche per sistemi multibody con contatti intermittenti.
iii
Acknowledgements
I would like to express my gratitude to a number of people, without whom this path would not have been the same. First of all, I wish to thank my supervi-sors for trusting me, and particularly Prof. Marco Gabiccini for his continuous encouragement over the last years.
Special thanks are also due to Prof. Moritz Diehl and his team for hosting me at the Systems Control and Optimization Laboratory during my stay at the University of Freiburg. I am really grateful that I had the opportunity to work in such a stimulating environment and to meet people that greatly contributed to my scientific growth. Particularly, I owe many thanks to Joris Gillis, from KU Leuven: without his invaluable advice and support, the last chapter of this thesis would not have been there at all.
Finally, I would also like to thank Manolo Garabini and Gian Maria Gasparri, from the Robotics Group of the Research Center “E. Piaggio” at the University of Pisa, for the hard work that led to the experimental validation of the optimized trajectories on a real robot.
Contents
Abstract i
Sommario ii
1 Introduction 1
1.1 Motivation . . . 1
1.2 Aim of the thesis . . . 3
1.3 Overview of the thesis . . . 3
2 Multibody systems dynamics 7 2.1 Geometric parameterization of rigid-body motion . . . 7
2.1.1 Rotational motion . . . 9
2.1.2 Generic rigid motion . . . 11
2.2 Differential Kinematics . . . 13 2.2.1 Velocity . . . 13 2.2.2 Acceleration . . . 15 2.3 Systems of forces . . . 19 2.4 Inertia . . . 20 2.4.1 Transformation . . . 21 2.4.2 Body representation . . . 22 2.4.3 Spatial representation . . . 22 2.5 Generalized momentum . . . 24 2.5.1 Body representation . . . 25 2.5.2 Spatial representation . . . 25 2.6 Equations of Motion . . . 26 2.6.1 Spatial form . . . 26 2.6.2 Body form . . . 29
3 Optimal control methods 33
3.1 Dynamic models . . . 33
3.2 Optimal control problems . . . 34
3.2.1 Optimal control of equations with invariants . . . 35
3.2.2 Periodic optimal control . . . 36
3.2.3 Periodic optimal control of uncertain systems . . . 37
3.3 Direct methods for optimal control . . . 41
3.3.1 Direct multiple shooting . . . 42
3.3.2 Direct collocation . . . 42
3.4 Numerical methods for time-integration . . . 43
3.4.1 Standard RK integrators . . . 43
3.4.2 Structure-preserving integrators . . . 45
3.5 Numerical methods for nonlinear optimization . . . 48
3.5.1 Interior-point methods . . . 49
3.5.2 Automatic differentiation . . . 50
4 Optimal control of an underactuated nonholonomic system 53 4.1 Dynamic model . . . 53
4.2 Trajectory optimization problem . . . 57
4.3 Comparison between different integration schemes . . . 59
4.3.1 Numerical results . . . 61
5 Optimal control through unscheduled contacts 73 5.1 Dynamic model . . . 74
5.2 Contact force model . . . 75
5.3 Trajectory optimization problem . . . 76
5.4 Computational strategy: two stage optimization . . . 78
5.4.1 Numerical results . . . 79
6 Optimal control through a scheduled sequence of contacts 85 6.1 Bipedal locomotion . . . 85
6.2 Problem formulation . . . 86
6.2.1 Contact formulation . . . 87
6.2.2 Periodicity . . . 89
6.2.3 Optimal control problem . . . 90
6.3 Case study: 6-DoF biped . . . 91
6.3.1 Model and Tasks description . . . 91
6.3.2 Numerical results . . . 92
CONTENTS vii
7 Robust optimal control of systems with contacts 99
7.1 Dynamic model . . . 100
7.1.1 Equations of Motion . . . 100
7.1.2 Contact phases . . . 101
7.1.3 Discontinuities at phase changes . . . 102
7.1.4 Periodicity . . . 102
7.2 Nominal optimal control problem . . . 103
7.3 Robustified optimal control problem . . . 104
7.3.1 Numerical solution . . . 107
7.4 Numerical results . . . 109
7.4.1 Nominal trajectory . . . 109
7.4.2 Robustified stable trajectories . . . 110
7.4.3 Simulation results . . . 113
8 Conclusions 119
Bibliography 120
Chapter
1
Introduction
1.1
Motivation
Robots are nowadays increasingly required to exit their laboratories and be able to operate in real-world environments, where these systems have the potential to make a significant contribution in high-risk operations. Specifically, robots are supposed to accomplish tasks that require human-like dexterity, but are at the same time too dangerous for humans to perform, for instance inspections in hos-tile or contaminated environments, demining in war zones, or search and rescue operations in areas affected by natural or nuclear disasters, such as the Fukushima power plant after the dramatic accident caused by the 2011 earthquake in Japan. In such contexts, legged robots appear among the most promising technologies to be employed, as they combine the potential to move on uneven terrains that challenge traditional wheeled vehicles and operate in environments originally de-signed for humans. However, as the 2015 DARPA Robotics Challenge [1] has clearly highlighted, state-of-the-art motion planning and control algorithms are still inappropriate to reliably employ robotic systems in the aforementioned haz-ardous applications. Indeed, notwithstanding a few remarkable exceptions [2, 3], the variety of motions legged robots are nowadays able to afford still suffers from a lack of dynamicity, efficiency and robustness.
In spite of the fact that the last decades have seen an impressive technical progress in the design of legged robots, also thanks to large investments from companies (among others Honda, Toyota, Kawada, Boston Dynamics), a general method for motion planning and control of such kind of systems is still missing. This is due to a number of critical factors: besides having many degrees of free-dom, that render their dynamics intrinsically challenging to control, these systems necessarily need to rely on unilateral contact forces in order to produce viable motions. Due to these issues, general-purpose motion planners appear inadequate
to plan effective trajectories for legged robots. Standard motion-planning tech-niques, based on probabilistic and sampling-based approaches [4–6], in fact, can determine solutions that are guaranteed to be only kinematically feasible. The obtained trajectories are therefore likely to be unnatural and unnecessarily com-plicated. Although more recent algorithms, such as RRT∗ [7], manage to alleviate this problem, by converging almost surely to an optimal solution, planners based on kinematic sampling cannot deal with highly dynamic motions.
Some traditional methods, such as the zero moment point (ZMP), proposed in [8], which is widely applied for generating walking gaits for legged robots, at-tempt to capture the dynamics of such complex systems by employing extremely simplified models, such as the linear inverted pendulum model (LIPM) [9, 10] or the spring loaded inverted pendulum (SLIP) [11]. Such methods appear therefore inherently unable to exploit the dynamic richness of the real system. Thus, the planned motions turn out to be conservative and inevitably inefficient. Particu-larly, while on the one hand the ZMP approach relies on full actuation in order to translate the trajectories generated for the center of mass of the robot into joint torques by means of inverse dynamics, on the other hand several studies [12, 13] have proven that exploiting the passive dynamics of the system leads to more efficient behaviors in terms of energy consumption.
Over the last decades, many other methods have been proposed to deal with the trajectory planning of legged robots; the reader is referred to [14] for an exhaustive overview. Interestingly, several research efforts, also assisted by in-creasingly powerful computational tools, have been directed towards generating efficient trajectories for legged robots by taking advantage of the application of direct optimal control methods [15]. Such approach has proven to be effective on simple models [16,17], but also to scale well to considerably complex models [18]. In this context, the hybrid nature of systems that interact with the surrounding environment through intermittent contact forces represents a challenge for numer-ical optimization methods, that cannot handle discontinuities. Special care must therefore be devoted to properly modeling the contact phenomenon, which is an inherently discontinuous one. Where possible and appropriate, one way around this problem consists in predefining a specific sequence of dynamic stages, differ-ing in the set of active contacts. However, other methods [19, 20] have recently been proposed to deal with problems in which a priori specifying the contact sequence to be exploited is impractical, e.g. due to the presence of many possible contact points.
When an approach based on numerical optimization is adopted in order to syn-thesize efficient motions for robotic systems in the context of loco-manipulation, the problem of stabilizing the planned motion in order to actually execute it on
1.2 Aim of the thesis 3
a real platform is usually addressed only after choosing the nominal trajectory, by means of local feedback stabilization [21]. On the other hand, numerical op-timization techniques themselves seem to offer good prospects for dealing with uncertainties. Given the constantly increasing computing power, the possibility to deal with the discrepancies between the theoretical model and the real sys-tem by on-line re-planning the optimal trajectory is now arising: preliminary investigations towards the application of model predictive control (MPC) have been carried out [22]. However, due to the complexity of the full-body dynam-ics, on-line optimizations are still too expensive to be performed in real time. Some research efforts have been devoted to speed up the on-line computations by combining numerical optimization approaches with simplifying assumptions on the dynamics of the system [23, 24]. An interesting alternative is represented by employing numerical optimal control in order to plan trajectories off-line that are inherently easy to stabilize: from this perspective, pioneering works [25, 26] have shown that considerations on the stability of the linearized dynamics along a nominal trajectory can be included in the optimization problem, so as to de-sign open-loop periodic gaits that are stable to external disturbances without any feedback.
1.2
Aim of the thesis
This thesis has a twofold purpose. On the one hand, it aims to investigate very basic problems concerning the application of advanced methods for multibody modeling to the planning of dynamic motions for multibody systems in general via numerical optimal control techniques.
On the other hand, the thesis is also focused in particular on the exploration of various methods that allow the trajectory optimization of a specific class of multibody systems: namely that in which the interactions between the system and the surrounding environment through intermittent contact forces play a ma-jor role. Specifically, through an application-oriented approach, the thesis aims at validating the effectiveness of direct optimal control as a means of planning trajectories for simple but challenging robotic applications of this sort, as well as at extending an already existing method for robust optimal control of nonlin-ear mechanical systems to the problem of planning trajectories for robots with intermittent contacts that can be effective also in the presence of stochastic un-certainties.
1.3
Overview of the thesis
The thesis is organized as follows. In Chapter 2 advanced methods based on Lie group theory, which are suitable for modeling the dynamics of systems of rigid
bodies in a three-dimensional space are presented. In Chapter 3, the methods from numerical optimal control literature that are widely employed over the whole thesis are reviewed.
The methods introduced in the first chapters are employed in the analysis presented in the remainder of the thesis, where numerical optimal control is ap-plied to the trajectory planning of several multibody systems (MBS), all chal-lenging for different reasons. In Chapter 4, the trajectory optimization of an underactuated, nonholonomic mechanical system is addressed: namely, a cart-pendulum system, consisting of a unicycle and of a cart-pendulum connected to it through a passive spherical joint. The description of the dynamics of this system involves the representation of the orientation of the spherical pendulum through a singularity-free parameterization. Specifically, the study presented, which is based on [27], focuses on the comparison between two non-minimal rotation pa-rameterizations and between different integration schemes that preserve, either by construction or by numerical stabilization, the invariants yielded in the dy-namics by employing such redundant parameterizations in the description of the rotational degrees of freedom (DoF) of MBS. Here, the main contribution is the assessment of the performance of structure-preserving integration schemes in the context of direct optimal control and its comparison with traditional numerical integration schemes.
The remainder of the thesis is focused in particular on the application of nu-merical optimal control methods to MBS with intermittent contacts, the target application being robotic locomotion. Specifically, in Chapter 5 the problem of underactuated MBS interacting with the environment through an a priori un-scheduled sequence of contacts is addressed. In this study, published in [28], an original computational strategy is devised, which aims at improving the com-putational efficiency of direct methods applied to the trajectory optimization of such kind of systems. The performance of the proposed method is evaluated in a simulated planar system, whose peculiarity is to be trivially underactuated.
To the aim of applying the developed algorithms to the control of real robots, the case of bipedal locomotion is then analyzed in further detail. Since for biped systems the scheduling of the contact phases to be exploited is trivially deter-mined, the dynamics of the system can be conveniently modeled by employing a multi-stage formulation, in which each dynamic stage corresponds to a different contact phase. Furthermore, steady-state bipedal locomotion may be regarded as a periodic optimal control problem, where the energy consumption is optimized for. In Chapter 6, this approach is applied to the optimization of walking and running gaits of a planar biped robot, actuated by series elastic actuators (SEA). Here, numerical optimal control is employed in order to let the robot properly
1.3 Overview of the thesis 5
exploit its dynamics in locomotion. In this context, the main contribution is the quantitative assessment of the energetic trade-off between rigid and series elastic actuation in the context of robotic locomotion, and specifically how it varies with the locomotion speed. Moreover, experimental results are also presented, that validate the actual feasibility of the optimized trajectories on a real robot.
In Chapter 7, the optimization of periodic walking gaits in the presence of stochastic disturbances is addressed. Specifically, to the aim of planning open-loop controlled trajectories that are effective also in the presence of uncertainties, an existing method for approximate robust optimal control of nonlinear periodic systems is applied to a very basic planar model of bipedal locomotion. In this context, the main contribution is in showing how such method appears as a suit-able tool to plan periodic trajectories that, in the presence of small disturbances, are guaranteed to be not only stable without feedback, but also robust to the violation of constraints, up to a certain confidence level.
Chapter
2
Multibody systems dynamics
In this chapter, an advanced approach from the literature, aimed at formulating the dynamics of three-dimensional MBS is presented in the first place. This geometric method allows for formulating the dynamics of arbitrarily complex systems of rigid bodies in a compact and systematic way [29, 30]. Specifically, such methodology benefits from the application of the Lie group theory [31] to the description of the motion of rigid bodies in space.
In such context, the contribution of this thesis is to provide a self-contained review of this method. The needed basic concepts from Lie group theory are introduced, particularly focusing on the advantages of employing such tools in the description of rigid-body kinematics and dynamics. Furthermore, the analysis presented relates the compact equations resulting from employing the Lie group formalism in the description of rigid-body motion to the equations traditionally employed in classical mechanics.
2.1
Geometric parameterization of rigid-body motion
A rigid body in a 3D space has 6 DoF. In order to identify the current position of a generic point p belonging to the rigid body, both the position and the orientation of the body with respect to a fixed (spatial) reference frame {S} need to be described. To this aim, at least two tools are required, see Figure 2.1:
• A three-dimensional position vector describing the position of the origin Ob of a body-fixed frame{B} with respect to the origin Osof the spatial frame {S};
• A rotation matrix Rsb describing the orientation of the body-fixed reference frame{B} with respect to the spatial frame of reference {S}. Rsb is a 3× 3 orthogonal matrix, whose elements represent the components of the unit
Figure 2.1: A rigid body in a 3D space.
vectors of frame{B} in coordinates of the frame {S}
Rsb= is· ib is· jb is· kb js· ib js· jb js· kb ks· ib ks· jb ks· kb
where the unit vectors i, j, k identify the x, y and z-direction of each frame, and the subscripts b, s the frames {B} and {S}, respectively. The orientation of a rigid body in a 3D space is not a vectorial quantity: it belongs to a nonlinear manifold, instead. A rotation matrix, in fact, verifies two important properties:
Rsb>Rsb= I det Rsb= 1 (2.1)
Given that 3D rotations are not vectorial quantities, Lie group theory is a suitable tool to be employed in the characterization of the pose of a rigid body in space. Lie groups and algebras
The basics of Lie theory are briefly introduced here. For a more detailed expla-nation of these concepts, the reader is referred to [29,32]. Given a set of elements G together with a binary operation ◦ : G × G → G, G is said to be a Lie group if all these seven properties hold:
1. a, b∈ G ⇒ a ◦ b ∈ G
2. a, b, c∈ G ⇒ a ◦ (b ◦ c) = (a ◦ b) ◦ c
3. An identity element e∈ G exists, such that a ◦ e = a, ∀a ∈ G 4. An inverse element a−1∈ G exists, such that a ◦ a−1= e,∀a ∈ G 5. G is a differentiable (smooth) manifold
2.1 Geometric parameterization of rigid-body motion 9
6. The binary operation◦ : G × G → G is infinitely differentiable 7. The inverse operation G→ G is infinitely differentiable
The tangent space g of the Lie group G at the identity element is called the Lie algebra of group G.
2.1.1 Rotational motion
The properties described in (2.1) identify the 3D rotation matrices as elements of the Lie group SO(3), i.e. the Special Orthogonal group. In fact, each element of the group is differentiable, and both an identity and an inverse element can be identified, that are, respectively, the 3× 3 identity matrix I and the transpose of the rotation matrix itself, i.e. R>
sb. The Lie algebra of the group SO(3) is so(3), whose elements are skew-symmetric matrices. Consider the rotation matrix of interest Rsb ∈ SO(3), that describes the orientation of {B} with respect to {S}. Let ω represent the angular velocity vector which identifies the rotational motion of the rigid body with respect to the fixed reference frame. Then, its “hat form” ˆ
ω belongs to so(3). In general, for any vector v ∈ R3, given a specific frame of reference in which v is represented by the components (vx, vy, vz), the hat form ˆ
v is a skew-symmetric matrix defined as follows
ˆ v = v0z −v0z −vvyx −vy vx 0 (2.2)
Moreover, it will be useful to write v∧w = ˆvw = v× w.
Each element belonging to the Lie group SO(3) can be expressed by an expo-nential mapping of the elements of the corresponding Lie algebra so(3). In fact, considering a pure rotational motion, the velocity of a generic point p belonging to the rigid body is described by the differential equation
˙p = ˆωp
whose integration, under the hypothesis that ω is constant over time, yields p(t) = eωtˆ p(0)
So, given a rigid body rotating about a fixed axis with angular velocity ω, the transformation of a point belonging to the body from its initial position to the current one can be described by the matrix
R(t) = eωtˆ
The nonlinear manifold SO(3) can be parameterized in several different ways [33]. Since this manifold is locally isomorphic to the three-dimensional Euclidean space
E3, it can be only locally parameterized by a chart of three independent coordi-nates, such as e.g. Euler angles. However, a minimal representation is not tailored to be employed in contexts where the orientation to be described spans a wide range. Euler angles suffer indeed from the inevitable presence of representation singularities. In order to avoid this problem, often referred to as gimbal lock, non-minimal representations of SO(3) must be employed, see [34] for an exhaustive overview. Non-minimal parameterizations overcome singularities by employing a higher-dimensional set of coordinates in the representation, such as the four parameters of a unit quaternion, or the nine parameters corresponding to all the elements of a rotation matrix. The latter parameterization will be referred to as direction cosine matrix (DCM). The introduced redundant coordinates must sat-isfy additional algebraic constraints in order for the representation to consistently describe physical rigid-body rotations. Specifically, a quaternion is required to have unit length, whereas a DCM is required to be an orthogonal matrix. Thus, non-singular parameterizations evolve on nonlinear manifolds themselves, i.e. re-spectively the Special Unitary group SU (2) for the unit quaternion and SO(3) itself for the DCM parameterization [35].
Quaternion parameterization
Quaternions are a number system that extends complex numbers to a four-dimensional space [36]. A quaternion q may be indicated as
q = (q0, qv) (2.3)
q consists of a scalar part q0 and a three-dimensional vector part qv, which can be written as qv = qiei, where ei represents each direction of the standard or-thonormal basis in a 3D space for i∈ {1, 2, 3}. The quaternion’s conjugate q∗ is defined as
q∗ = (q0,−qv) (2.4)
Let the product of two quaternions q and p be defined as follows
q· p = (q0p0− qv>pv, q0pv+ p0qv+ qv× pv) (2.5) the square root of the product of a quaternion and its conjugate is called quater-nion norm, here indicated askqk
kqk2 = q· q∗= (q2
0+ qv>qv, 0) (2.6)
Unit quaternions belong to the Special Unitary group SU (2) and therefore satisfy the unit length condition
kqk − 1 = q
q2
2.1 Geometric parameterization of rigid-body motion 11
A unit quaternion can parameterize every rotation matrix Rsb ∈ SO(3) that describes the orientation of a body-fixed reference frame {B} with respect to a fixed reference frame{S}, according to the following relation [37]
Rsb(q) = 1 4Gs(q)Gb(q) > (2.8) with Gs(q) = 2 −q1 q0 −q3 q2 −q2 q3 q0 −q1 −q3 −q2 q1 q0 Gb(q) = 2 −q−q12 −qq03 qq30 −qq12 −q3 q2 −q1 q0 The quaternion dynamics can be expressed as
˙q = 1 4Gb(q)
>ωb (2.9)
where ωb indicates the representation of the angular velocity vector ω in coordi-nates of the body-fixed reference frame. It can be shown that Eq. (2.9) preserves the norm kqk of the quaternion over time.
DCM parameterization
Parameterizing the rotation Rsb via a DCM description consists in describing such rotation by using all the nine elements of Rsb as parameters. Such elements represent the direction cosines of each axis of the body-fixed reference frame{B} with respect to the fixed reference frame {S}, see [38]. In order for a matrix R ∈ R3×3 to be a valid DCM, it must belong to SO(3), i.e. it must satisfy the orthonormality condition
R>R− I = 0 (2.10)
It can be easily verified that orthonormality is preserved over time through the dynamics of the DCM, which reads
˙
R = Rˆω (2.11)
Here, ˆω ∈ so(3) indicates the skew-symmetric matrix form of the angular velocity ω, expressed in the body-fixed reference frame.
2.1.2 Generic rigid motion
A generic rigid motion can be identified by a single object: the homogeneous transformation gsb. Such an object can be properly defined in such a way that it contains the information regarding both the position and the orientation of the
body-fixed reference frame {B} with respect to the spatial reference frame {S}. gsb can be represented by a 4× 4 matrix, defined as follows
gsb= Rsb dsb 01×3 1 (2.12) where dsb is the spatial representation of the vector describing the position of the origin of the frame{B} with respect to the origin of frame {S}. The transforma-tion gsbacts on homogeneous vectors, i.e. vectors having an additional component whose function is to indicate what kind of vector we are dealing with. Therefore, a homogeneous vector describing a physical quantity defined in a three-dimensional space belongs to R4, instead of R3: the additional component is 1 in case of posi-tion vectors and 0 otherwise. Given that dsb ∈ R3 and Rsb∈ SO(3), gsb belongs to the Lie group SE(3), i.e. the Special Euclidean group. The identity element of the group is the 4× 4 identity matrix I, whereas its inverse element of a generic transformation matrix gsb is defined as
gsb−1= R> sb −R>sbdsb 01×3 1 (2.13) The Lie algebra of SE(3) is se(3), whose elements are called twists and iden-tify a generic rigid motion in a 3D space. A twist might be represented, in an homogeneous form, by a 4× 4 matrix
ˆ ξ = ˆ ω v 01×3 0 (2.14) where ω describes the angular velocity of the rigid body and v its linear velocity. In an alternative form, the same twist can also be represented by a vector ξ∈ R6, having components ξ = v ω (2.15) Similarly to what was shown for the case of the group SO(3) and its algebra so(3), there is also an exponential mapping relating se(3) to SE(3). In fact, in a generic rigid motion, the velocity of a point p belonging to the rigid body can be written as
˙p(t) = ˙q + ω× (p − q) = ω × p + ˙q − ω × q (2.16) where q is a point belonging to the same rigid body as p. Let us define
v = ˙q− ω × q
then, Eq. (2.16) may be represented in homogeneous form as ˙p 0 = ˆ ω v 01×3 0 p 1 = ˆξ p 1 (2.17)
2.2 Differential Kinematics 13
which might be written in a more compact form as ˙¯
p(t) = ˆξ ¯p(t)
where ¯p = [p, 1]>indicates the homogeneous representation of the position vector p. When ξ is not time dependent, integration yields
¯
p(t) = eξtˆp(0)¯
For a more detailed explanation the reader is referred to [29].
2.2
Differential Kinematics
In this section, the physical meaning of the description through tools from Lie group theory of the velocity and the acceleration of a rigid body moving in space will be analyzed.
2.2.1 Velocity
A generic rigid motion can be described by twists, that are objects belonging to the Lie algebra se(3). Suppose one wants to describe the motion of a rigid body, given two different reference frames: the body-fixed frame {B} and the spatial frame {S}. The homogeneous transformation relating the two frames is gsb. The aim is now to study the twist Vsb, that represents the velocity of {B} with respect to {S}. A twist might be regarded as an operator that maps homogeneous vectors representing the position of points belonging to the rigid body into the homogeneous expressions of their velocity, as relation (2.17) clearly shows. Starting from this consideration, a general expression for Vsb can easily be derived, either in spatial or in body coordinates.
Consider a generic point p belonging to the body, whose homogeneous coor-dinates are indicated by ¯ps and ¯pb in spatial and body coordinates, respectively. These two representations of the same point are related as follows
¯
ps= gsbp¯b (2.18)
In order to obtain an analytical expression for the twist, this expression can be differentiated. Bearing in mind that ¯pb is constant, as both p and {B} belong to the same rigid body, we have
˙¯
ps= ˙gsbp¯b
From this relation, the representation of the twist Vsb in the spatial reference frame can be obtained
˙¯
In fact, since a twist is an operator that maps homogeneous representations of points into their velocities, (2.19) defines the spatial representation of the twist Vsb in matrix form b Vsbs = ˙gsbg−1sb = ˙ RsbR>sb − ˙RsbR>sbdsb+ ˙dsb 01×3 0 (2.20) Let us analyze all the elements of this block matrix. The first term provides the well known expression of the rotational velocity in the spatial reference frame
ˆ
ωs= ˙RsbR>sb
whereas the second one gives the following expression for the linear velocity vs vs=− ˙RsbR>sbdsb+ ˙dsb = ˙dsb− ˆωsdsb (2.21) Since ˙dsb is the linear velocity of the origin of the body fixed reference frame Ob with respect to the spatial reference frame, expressed in {S} coordinates, Eq. (2.21) is equivalent to the formula relating the velocity of two points fixed on the same rigid body
vs= vOb+ ω
s× O
bOs (2.22)
Therefore, vs represents the linear velocity of a point O0
s (notice that it can also be imaginary) belonging to the rigid body, which instantaneously coincides with Os.
The expression of the representation of twist Vsb in the body-fixed reference frame can be derived, as well. In order to do so, it will be convenient to define the adjoint action of the Lie group SE(3) on its Lie algebra se(3). Given a homogeneous transformation gsb ∈ SE(3), an adjoint transformation associated to gsb can be defined. Specifically, this transformation can be identified by the following linear operator
Adgsb = Rsb dˆsbRsb 03×3 Rsb (2.23) which is an invertible operator, with inverse element defined as
Ad−1g sb = Rsb> −R> sbdˆsb 03×3 R>sb .
Ad : SE(3)× se(3) → se(3) maps elements belonging to the twist representation of se(3), expressed in a certain coordinate system, to their representation in another coordinate system. Specifically, given the representation ξy of the twist ξ in a generic reference frame{Y } and the homogeneous transformation gxy, via
2.2 Differential Kinematics 15
Adgxy, ξ
x can be computed, that is the representation of ξ in the reference frame {X}. In fact, the following relation holds
ξx= Adgxyξ
y (2.24)
It is easy to prove that the equivalent of this relation in homogeneous form is the following
ˆ ξx= g
xyξˆyg−1xy (2.25)
Therefore, between the spatial and the body-fixed representation of the twist Vsb describing the motion of a rigid body in space, the following relation holds
b
Vsbb = gsb−1Vbsbsgsb (2.26)
from which, by substituting (2.20), it is straightforward to derive an expression for the body-fixed representation Vb
sb in its matrix form b Vb sb = gsb−1˙gsb = R>sbR˙sb R>sbd˙sb 01×3 0 (2.27) Again, the first matrix block is the expression of the angular velocity in body coordinates, i.e.
ˆ
ωb = Rsb>R˙sb (2.28)
Let us now analyze the resulting expression of the linear velocity vb
vb = R>sbd˙sb (2.29)
Since ˙dsb is the linear velocity of the origin of the body-fixed reference frame Ob with respect to the spatial reference frame, expressed in {S} coordinates, Eq. (2.29) means that vb represents the linear velocity of the origin of the body reference frame Ob with respect to the fixed reference frame, expressed in {B} coordinates.
Thus, with reference to Eq. (2.15), it has been shown that the first three components of the twist represent the linear velocity of a point belonging to the rigid body that instantaneously coincides with the origin of the used reference frame. The last three components describe the angular velocity of the body in coordinates of the same reference frame.
2.2.2 Acceleration
The expressions obtained in Section 2.2.1 for the representation of a twist de-scribing the velocity of a rigid body in space can be conveniently differentiated in order to obtain descriptions of the acceleration of the body itself with respect to a fixed reference frame.
It is worth mentioning that when we differentiate a twist, what we obtain is again a twist-type object. Let us go into further detail to understand what the components of this new twist represent. In Section 2.2.1, the meaning of both the spatial and the body velocity were clarified: namely, the first three components of a twist describe the linear velocity of the point of the body that instantaneously coincides with the origin of the reference frame, whereas the last three components describe the angular velocity of the rigid body.
The time derivative of the spatial velocity might be obtained by differentiation and reads ˙ Vsbs = ˙vs ˙ωs (2.30) The last three components clearly describe the angular acceleration of the body with respect to a fixed reference frame, expressed in{S} coordinates. The inter-pretation of the first three components, on the other hand, is not so straightfor-ward. Let us take a step back and differentiate Eq. (2.21), obtaining
˙vs= ¨dsb− ˆ˙ωsdsb− ˆωsd˙sb (2.31) Notice that, as Osis a fixed point, ˙dsb and ¨dsb represent respectively the absolute linear velocity and acceleration of the point Ob with respect to an inertial frame, expressed in coordinates of the reference frame{S}. By inverting Eq. (2.21), ˙dsb can be expressed by
˙
dsb= vs+ ˆωsdsb (2.32)
Substituting this into Eq. (2.31), we obtain ˙vs= ¨d
sb+ ˆ˙ωs(−dsb) + (ˆωs)2(−dsb)− ˆωsvs (2.33) The first three terms of the right hand side of this equation would represent the acceleration of Os in {S} coordinates, if such point belonged to the rigid body. Since there is an additional term, namely−ˆωsvs, ˙vs does not represent the linear acceleration of the point O0
sof the body instantaneously coinciding with the origin of the reference frame, as one could have supposed.
The additional term takes into account the “covariant derivative” of the veloc-ity, which is due to the fact that we are providing an Eulerian description of the velocity field. In fact, the linear velocity is always described by vs, i.e. the linear velocity of the origin of the spatial reference frame Os. It is worth highlighting that this point is a fixed one, it does not belong to the rigid body. From this fixed point, we describe the velocity field of a moving rigid body. At different instants, the linear velocity of the rigid body vs will always be calculated in the same fixed point Os.
2.2 Differential Kinematics 17
It is important to note that the point Os0 of the rigid body that instanta-neously coincides with Os is not the same, instead, but it changes over time. This variation causes the additional term in the derivative ˙vs, which in fact rep-resents the instantaneous evolution of the physical quantity vs over time. What does this means in practice? Consider a point Os0 whose position instantaneously coincides with the fixed point Os, but actually belongs to the rigid body. As Os0 and Obbelong to the same rigid body, their linear velocities are related as follows
vO0
s = vOb + ω× ObO
0 s Differentiation with respect to time yields
˙vO0
s = ˙vOb+ ˙ω× ObO
0
s+ ω× (vO0
s − vOb) (2.34)
Now, taking into account that O0
s and Ob belong to the same rigid body, the following relation holds
vO0
s − vOb = ω× ObO
0 s which leads to the well known formula
˙vO0
s = ˙vOb+ ˙ω× ObO
0
s+ ω× (ω × ObOs0)
Now, compare Eq. (2.34), expressing the acceleration of point O0s, which actually belongs to the body, with Eq. (2.31), expressing the time derivative of the quantity vs. They are apparently the same, except for the last term. In fact, the term vO0
s− vOb only takes into account the velocity of point Ob with respect to point
O0s, and is therefore a relative velocity, whereas ˙dsb is the absolute velocity of point Ob. The explanation of this difference appears somehow obvious, as point Os, conversely to O0s, is fixed: in this case the relative and the absolute velocity of point Ob are the same.
Consider now the time derivative of the body velocity, i.e. the same physical quantity ˙Vsb expressed in a different reference frame{B}
˙ Vsbb = ˙vb ˙ωb (2.35) In order to understand what the components of this vector stand for, first notice that {B} is moving, so in a general case, it is not an inertial reference frame, as {S} is. Thus, we have to bear in mind the important relation holding between the derivative of a vector with respect to an inertial reference frame and its derivative with respect to a non-inertial one. Specifically, given a generic vector u ∈ R3, between its spatial and body-fixed representations, the following relation holds
The derivative of u with respect to the non-inertial reference frame{B} is called the relative derivative, indicated here as ˙ub. The spatial representation of this vector will be indicated as ˙us
b. whereas its body representation will be indicated as ˙ub
b. The vector describing the derivative of u with respect to the inertial reference frame{S}, which is the absolute derivative, will be indicated as ˙us, and its representation in the spatial and the body reference frame will be referred to respectively as ˙us
s and ˙ubs. Suppose one wants to calculate ˙uss. Differentiation of Eq. (2.36) yields
˙uss= ˙Rsbub+ Rsb˙ubb = ˆωsR
sbub+ Rsb˙ubb
The representation of this vector in the body-fixed reference frame can be easily obtained multiplying by the rotation matrix R>sb
˙ubs= R>sbωˆsRsbub+ R>sbRsb˙ubb = ˆωbub+ ˙ub
b (2.37)
This is the important relation holding between the relative and the absolute derivative of a vector. Notice that in the special case in which the vector u to be differentiated is the angular velocity ω, the absolute and the relative derivative coincide.
Back to the analysis of Eq. (2.35), the last three coordinates ˙ωb simply rep-resent the angular acceleration of the body, expressed in {B} coordinates. As concerns the first three coordinates, instead, an expression of ˙vb might be ob-tained by differentiating Eq. (2.29)
˙vb = ˙R>
sbd˙sb+ R>sbd¨sb
Now, multiply by the identity term RsbR>sb = I and then exploit the fact that (ˆωb)> = ˙R>
sbRsb, as follows
˙vb= R>sbd¨sb+ ˙R>sbRsbR>sbd˙sb
= R>sbd¨sb− ˆωbvb (2.38)
Hence, ˙vb represents the linear acceleration of the point O
b plus an additional term, −ˆωbvb. This term is due to the fact that the reference frame we are now using, i.e. {B}, is not an inertial one. In order to better understand what this means, this expression might be compared with the known relation (2.37). The term R>sbd¨sb represents the absolute linear acceleration of point Ob, expressed in {B} coordinates, whereas the last term is analogue to ˆωb
sbub, but with a minus sign. Thus, ˙vb represents the linear acceleration of point O
b with respect to the non-inertial reference frame {B}, expressed in body coordinates. ˙vb is therefore a relative acceleration.
2.3 Systems of forces 19
Lie brackets
In order to differentiate twists, it will be useful to introduce another Lie operator, namely the adjoint action of the Lie algebra se(3) on itself. This operation is a binary one, mapping two elements belonging to se(3) to an element belonging to se(3), as well. Given a twist ξ1 ∈ se(3), the operator adξ1 is defined as
adξ1 = ˆ ω1 vˆ1 03×3 ωˆ1 (2.39) which is a 6× 6 skew-symmetric matrix. By applying this operator to another element of se(3), say ξ2, ξ1 acts on ξ2 as follows:
adξ1ξ2= ˆ ω1 ˆv1 03×3 ωˆ1 v2 ω2 (2.40) The result of this operation belongs to the Lie algebra se(3), too. The homoge-neous form of this operation is called Lie bracket and has the following expression
[adξ1ξ2]
∧
= ˆξ1ξˆ2− ˆξ2ξˆ1
The two operators that were just defined are linked by the following relation Adeξtˆ = e adξt (2.41) which is equivalent to adξ = dtd Adeξtˆ t=0 .
2.3
Systems of forces
From a Lie theory-based perspective, a system of forces acting on a rigid body may conveniently be described by a wrench. Wrenches are objects living in a space which will be referred to as se∗(3). This space is dual to se(3), the space where twists live. Similarly to twists, also the wrenches have both a matrix and a vector representation.
Namely, in a generic frame of reference{A}, a wrench w might be represented by a 6D vector wa, defined as follows
wa= Fa Ma Oa (2.42) Here, the first three components of the vector represent the three components of the resultant force F acting on the rigid body, expressed in{A} coordinates. The last three coordinates represent the three components of the resultant moment MOaabout the origin Oaof the reference frame, also expressed in{A} coordinates,
Figure 2.2: System of forces acting on a rigid body.
The same wrench can also be expressed in homogeneous form, through a 4× 4 matrix ˆ wa= ˆ Fa Ma Oa 01×3 0 (2.43) As they live in a space which is dual to se(3), wrenches can be transformed from one representation to another using the inverse of the transpose of the operator Adg. Specifically, given the representation wb of a wrench in the body-fixed reference frame{B}, its spatial representation may be obtained through
ws= Ad−>gsb wb (2.44) The operator Ad−>g sb is defined as follows Ad−>gsb = Rsb 03×3 ˆ dsbRsb Rsb (2.45) Also a relation analogue to (2.41) holds, which is
Ad−> eξtˆ = e − ad> ξ t (2.46) where ad∗ξ =− ad>ξ = ˆ ω 03×3 ˆ v ωˆ
For brevity, hereinafter the operator Ad−> will also be referred as Ad∗.
2.4
Inertia
In this section, a particular tensor will be introduced and analyzed, that is called “generalized inertia tensor” and will be useful to describe the inertial properties of a body while using the spatial vector algebra. Once a particular reference frame is chosen to express it, this tensor can be represented by a 6× 6 matrix. The generalized inertia matrix can be seen as a linear operator mapping twists into
2.4 Inertia 21
wrenches. In fact, through the multiplication of the twist representing the velocity of a body by its generalized inertia matrix, a vector is obtained that represents the generalized momentum of the body. This physical quantity belongs to se∗(3). For a more detailed explanation of these concepts, the reader is referred to [39].
Let us introduce the physical quantity M ∈ R6×6. We will refer to M as the generalized inertia tensor. This tensor is useful to describe the inertial properties of a rigid body, i.e. both its mass and its inertia tensor. The representation of M in a body fixed reference frame {G}, whose origin coincides with the center of mass G of the body, will be indicated as Mg and is expressed by the following block matrix Mg = mI3×3 03×3 03×3 JgG (2.47) where m is the mass of the body and JgG∈ R3×3is the inertia matrix of the body about the point G, expressed in{G} coordinates. It is worth noticing that all of the elements of Mg are constant, as the reference frame {G} is body-fixed.
2.4.1 Transformation
Given the representation of the generalized inertia tensor in a specific reference frame, its representation in another reference frame can be obtained through an appropriate transformation. For example, given the homogeneous transformation gsb and Mb, which is the generalized inertia matrix expressed in the reference frame{B}, the representation of M in the reference frame {S} can be calculated as follows
Ms= Ad−>gsb MbAd−1gsb (2.48)
This transformation is perfectly consistent with the expression of the kinetic energy T . In fact, by definition, T can be written as follows
T = 1 2(V b sb)>MbVsbb = 1 2(V s sb)>MsVsbs (2.49) where both the generalized inertia matrix and the twist Vsb can be represented either in their body or spatial form. As the relation holding between the two representations of Vsb is known, by substituting
Vsbb = Ad−1gsbVsbs what we obtain is T = 1 2(V s sb)>Ad−>gsb M bAd−1 gsbV s sb (2.50)
This proves Eq. (2.48). This relation can be exploited to deduce the expression of the tensor M in any reference frame, as the definition of Mg in the reference frame {G} has already been introduced.
2.4.2 Body representation
Consider a generic body-fixed reference frame {B}. The homogeneous transfor-mation relating this reference frame to{G} will be indicated by gbg. As both the reference frames considered are fixed on the same rigid body, gbg is constant over time. The aim is to calculate the explicit expression Mb of the generalized inertia tensor in a generic body-fixed reference frame{B}. From Eq. (2.48)
Mb = Ad−>gbg MgAd−1gbg = Rbg 03×3 ˆ dbgRbg Rbg mI3×3 03×3 03×3 JgG " R>bg −R>bgdˆbg 03×3 R>bg #
After some calculations, we get Mb =
"
mRbgR>bg −mRbgR>bgdˆbg m ˆdbgRbgRbg> RbgJgGRbg> − m ˆdbgRbgR>bgdˆ>bg
#
This expression might be simplified, by considering that that RbgR>bg = I and that the well known relation Jb
G = RbgJgGR >
bg holds, such that
Mb = " mI3×3 −m ˆdbg m ˆdbg JbG+ m ˆdbgdˆ>bg # = " mI3×3 −m ˆdbg m ˆdbg JbOb # (2.51) Notice that all of the elements of the matrix Mb are constant, i.e. they are not time dependent.
2.4.3 Spatial representation
Similarly, an expression for Ms, i.e. the matrix representing the generalized inertia tensor in a spatial reference frame {S}, can be deduced. In fact, from transformation (2.48), by choosing {G} as the body-fixed reference frame, we obtain Ms= " mI3×3 −m ˆdsg m ˆdsg JsG+ m ˆdsgdˆ>sg # = " mI3×3 −m ˆdsg m ˆdsg JsOs # (2.52) Notice that in this case, as the body is moving, while {S} is a fixed reference frame, the elements of the matrix Ms are not constant, but they change over time. In fact, both the translation vector dsg and the inertia matrix JsOs are time dependent. It will be useful to compute the time derivative of this matrix, ˙Ms. Time derivative
Relation (2.48) holds. Furthermore, it was already remarked that the matrix Mb is constant, whereas gsb is not (therefore the adjoint actions Ad−1gsb and Ad
−> gsb are
time dependent). Therefore, in order to determine ˙Ms, the time derivative of both Ad−1gsb and Ad−>gsb need to be calculated. Let us start with the computation
2.4 Inertia 23
of the time derivative of Ad−1gsb. It is convenient to exploit the definition of this operator in its matrix form
Ad−1gsbX∧ =hAdg−1 sb X
i∧
= gsb−1Xgb sb Supposing that vector X is not time dependent, this implies
d dt
Adg−1sbX∧ = ˙g−1sb Xgb sb+ g−1sb X ˙gb sb (2.53) Now, in order to calculate ˙g−1sb , the following relation might be differentiated
¯ pb = g−1sb p¯s Obtaining ˙gsb−1p¯s+ g−1 sb p˙¯ s= 0 ˙g−1sb p¯s+ gsb−1˙gsbp¯b= 0 ˙gsb−1p¯s+ gsb−1˙gsbg−1sb p¯ s= 0
From relation (2.20), it can be derived that ˙gsb−1=−gsb−1Vbsbs. By substituting this relation into (2.53), we obtain
d dt
Ad−1gsbX∧ =−g−1sb VbsbsXb− bX bVsbsgsb (2.54) In twist form, this is equivalent to
d dt Ad −1 gsb =− Ad−1g sbadVsbs (2.55)
In addition, the following relation holds d dt Ad−>g sb = d dt Ad −1 gsb >
Therefore, from (2.55) also the derivative of the operator Ad−>gsb can be deduced d dt Ad−>gsb= − Ad−1gsbadVs sb > =− ad>Vs sbAd −> gsb (2.56)
Now, there are all the elements necessary to calculate the derivative of the spatial representation of the generalized inertia tensor with respect to time.
˙ Ms= d dt Ad−>gsbMbAdg−1sb+ Ad−>gsb Mb d dt Ad −1 gsb =− ad>Vs sbAd −> gsb M bAd−1 gsb− Ad −> gsb M bAd−1 gsbadVsbs = ad∗Vs sbM s− Msad Vs sb (2.57)
2.5
Generalized momentum
As already mentioned, the generalized inertia tensor can be regarded as an appli-cation mapping twists into wrenches. In fact, the vector obtained by multiplying a twist representing the velocity of a body by its generalized inertia matrix rep-resents the generalized momentum of the body itself. This vector belongs to se∗(3), i.e. the space where wrenches live, and it is obtained by stacking the linear momentum Q and the angular momentum h together. Let us recall that, by definition, the angular momentum of a body about a point O is defined as
hO= Z V
ρ (OP × vP) dV (2.58)
where P is any point belonging to the body, ρ is its density and V its volume. Starting from its definition, this vector can be equivalently rewritten as follows
hO= Z V ρ OP × (vP − vO+ vO) dV = Z V ρ OP × (vP − vO) dV + Z V ρ (OP × vO) dV
Considering that vP − vO is the relative velocity of point P with respect to point O, the relative angular momentum about the point O can be defined as
hrelO = Z V
ρ OP × (vP − vO) dV
Exploiting both this definition and the definition of the centre of mass G, hOcan be expressed as follows
hO = hrelO + m(OG)× vO
Moreover, assuming that P and O belong to the same rigid body, their relative velocity can be expressed as vP − vO = ω× (OP ), from which it follows that hrel
O = JOω. Substituting this expression of hrelO in the previous formula, we have
hO= JOω + m(OG)× vO (2.59)
The vector hO may be represented in any reference frame: whatever frame is employed, its representation consists in a vector belonging to R3. In the following, the body and the spatial representations of the generalized momentum will be analyzed in detail.
2.5 Generalized momentum 25
2.5.1 Body representation
When both the generalized inertia tensor and the twist Vsb are expressed in the body-fixed reference frame{B}, by definition, the generalized momentum is
MbVsbb = " mI3×3 −m ˆdbg m ˆdbg JbOb # vb ωb = " m(vb− ˆd bgωb) JbObω b+ m ˆd bgvb # (2.60) What does this expression stand for? The first three components express the linear momentum Q of the body, in {B} coordinates. In fact, it was already shown that vb represents the linear velocity of the point O
b of the rigid body. The velocity of the centre of mass G can be written as a function of vb, due to the well known formula relating the velocity of two points belonging to the same rigid body. Therefore, we have
Qb = mvGb = m(vObb+ ωb× (ObG)b) = m(vb− ˆdbgωb) where vb
G is the linear velocity of the centre of mass G of the body. Let us now analyze the last three components of the vector MbVb
sb. It can be shown that those are equal to the angular momentum hb
Ob, i.e. the angular
mo-mentum about the point Ob, expressed in {B} coordinates. In fact, by applying the relation (2.59), hb
Ob can be calculated as follows
hbOb = JbObω b+ m(O bG)b× vObb = J b Obω b+ m ˆd bgvb
This expression perfectly coincides with the last three components of the vector in Eq. (2.60). This proves that the generalized momentum of a body, expressed in the body fixed reference frame {B} is
MbVsbb = Qb hb Ob (2.61) 2.5.2 Spatial representation
Let us now focus on the spatial representation of the generalized momentum. In this case, both the generalized inertia and the velocity of the body are expressed in {S} coordinates. So, by definition, the generalized momentum has the following expression MsVsbs = " mI3×3 −m ˆdsg m ˆdsg JsOs # vs ωs = " m(vs− ˆd sgωs) JsOsω s+ m ˆd sgvs # (2.62) It was already shown that vsrepresents the linear velocity of a point O0
sbelonging to the rigid body that instantaneously coincides with the fixed point Os. Because
of the formula relating the velocities of two points belonging to the same rigid body, the following relation holds
Qs= mvs
G= m(vOs0 s+ ω
s× (O
sG)s) = m(vs− ˆdsgωs)
In order to clarify the physical meaning of the last three components of (2.62), let us analyze the angular momentum of the rigid body about the point Os. Since instantaneously, O0sand Oscoincide, the formula (2.59) may be employed in order to express the angular momentum as follows
hsOs = hsO0 s = J s O0 sω s+ m(O0 sG)vsO0 s = Js Osω s+ m ˆd sgvs
Therefore, the spatial expression of the generalized momentum is MsVs sb = Qs hs Os (2.63)
2.6
Equations of Motion
The dynamics of a rigid body can be described by the so-called Equations of Mo-tion (EoM). Specifically, the translaMo-tional and the rotaMo-tional dynamics of the rigid body may be described, respectively, by Newton’s second law and Euler’s equa-tions. When using the spatial vector algebra, these equations can be written in an extremely compact way, using the means from Lie group theory introduced so far, by simply differentiating the generalized momentum of the body and equating it to the total external wrench acting on the body itself.
In this section the EoM, expressed either in a spatial or a body fixed reference frame, will be derived and analyzed. Particularly, a comparison with their 3D-vector algebra equivalent will be presented, which the reader is likely to be more familiar with.
2.6.1 Spatial form
When a spatial frame of reference is employed to represent the EoM, those can be expressed as a 6D-vector equation as follows
d dt(M
sVs
sb) = ws (2.64)
In fact, considering the relation (2.63), this equation is perfectly equivalent to the Newton-Euler equations. This clearly appears when writing Eq. (2.64) in an expanded form d dt Qs hs Os = Fs Ms Os
2.6 Equations of Motion 27
in which the last three components represent indeed the Euler’s equations, that ensure the moment balance about the fixed point Os.
To the aim of analyzing the terms of Eq. (2.64) into further detail, let us start by calculating the derivative of the generalized momentum, expressed in spatial form
d dt(M
sVs
sb) = MsV˙sbs + ˙MsVsbs Here, ˙Ms may be expressed through (2.57), obtaining
d dt(M sVs sb) = MsV˙sbs + ad ∗ Vsbs MsVsbs − MsadVs sbV s sb = MsV˙sbs + ad∗Vs sbM sVs sb
which leads to the following expression for the EoM in spatial form MsV˙s sb+ ad ∗ Vs sbM sVs sb = ws (2.65)
Let us now analyze the terms of this spatial vector equation. In an expanded block-matrix form, Eq. (2.65) can be written as follows
" mI3×3 −m ˆdsg m ˆdsg JsOs # ˙vs ˙ωs + ˆ ωs 0 3×3 ˆ vs ωˆs m(vs+ ˆωsd sg) JsOsω s+ m ˆd sgvs = Fs Ms Os (2.66) In order to compare this equations to the 3D-vector form of the EoM, we are going to study the first three equations separately from the last three.
Newton’s second law
The first three components of the EoM (2.65) represent Newton’s second law. In fact, going through the calculations of the upper blocks of Eq. (2.66), the following 3D-vector equation is obtained
m ˙vs− m ˆdsg˙ω + ˆωsm(vs+ ˆωsdsg) = Fs ˙vs can be replaced by its expression (2.33)
mhd¨sb+ ˆ˙ωs(−dsb) + (ˆωs)2(−dsb)− ˆωsvs i − m ˆdsg˙ω + mˆωs(vs+ ˆωsdsg) = Fs Simplification leads to mhd¨sb+ ˆ˙ωs(−dsb+ dsg) + (ˆωs)2(−dsb+ dsg) i = Fs From the definition of dsb and dsg, the following relation holds
from which we obtain
mhd¨sb+ ˆ˙ωs(ObG)s+ (ˆωs)2(ObG)s i
= Fs (2.67)
As already remarked before, ¨dsb represents the acceleration of the point Ob be-longing to the rigid body with respect to the fixed point Os, expressed in {S} coordinates. Therefore, due the formula that relates the accelerations of two points belonging to the same rigid body, the term inside the square bracket in Eq. (2.67) represents the acceleration aG of the centre of mass G of the body, expressed in{S} coordinates.
This proves that the first three scalar equations in (2.65) simply represent Newton’s second law, expressed in the spatial reference frame
d dtQ
s= mas
G= Fs (2.68)
Euler’s equations
The last three components of the EoM (2.65) represent the rotational dynamics of the body, through the well known Euler’s equations. Consider the lower blocks in Eq. (2.66). Going through the calculations, the following 3D-vector equation is obtained m ˆdsg˙vs+ JsOs˙ω s+ ˆvsm(vs+ ˆωsd sg) + ˆωs(JsOsω s+ m ˆd sgvs) = MOss
˙vs can be replaced by its expression (2.33), which yields m ˆdsg h ¨ dsb+ ˆ˙ωs(−dsb) + (ˆωs)2(−dsb)− ˆωsvs i + +JsOs˙ω s+ ˆωs JsOsω s+ mˆvsωˆsd sg+ mˆωsdˆsgvs= MOss
Rearranging the terms, this equation can also be written as m ˆdsg h ¨ dsb+ ˆ˙ωs(−dsb) + (ˆωs)2(−dsb) i + JsOs˙ω s+ ˆωs JsOsω s+ mˆvsωˆsdsg+ ˆωsdˆsgvs+ ˆdsgˆvsωs = MOss (2.69)
The term in the round brackets is null because of the Jacobi identity. Let us now focus on the term inside the square brackets. Since dsbis the position vector OsOb expressed in{S} coordinates, ¨dsb is the acceleration aOb of the point Obbelonging
to the body with respect to the fixed point Os, expressed in {S} coordinates, as well. Therefore, this term can also be written in the following form
¨
dsb+ ˆ˙ωs(−dsb) + (ˆωs)2(−dsb) = asOb+ ˙ω
s
× (ObOs)s+ ωs× (ωs× (ObOs)s) For the formula relating the accelerations of two points belonging to the same rigid body, this expression is equivalent to the spatial representation of the acceleration
2.6 Equations of Motion 29
aO0
s, i.e. the acceleration of a point O
0
sof the body instantaneously coinciding with the fixed point Os. It is worth noticing that, as in the instant we are considering O0
s≡ Os, instantaneously also the following relations hold JO0
s = JOs
dsg= (OsG)s= (O0sG)s Therefore, Eq. (2.69) can be rewritten as follows
JsO0 s˙ω s+ ωs × JsO0 sω s+ m(O0 sG)s× asO0 s = MOs (2.70)
Using the expression (2.59) of the angular momentum, it can be proven that the 3D-vector equation we are considering is equivalent to Euler’s equations, expressed in the spatial reference frame. In fact
d dth s Os = d dt hrelO0 s s + m(Os0G)s× as O0 s = MOs (2.71) 2.6.2 Body form
The EoM equal the total wrench acting on the body to the time derivative of the generalized momentum with respect to an inertial reference frame. This relation can be expressed in components of a generic reference frame, e.g. a body-fixed one, but in this case attention must be paid to the fact that the reference frame employed might be a non-inertial one. Therefore, to avoid ambiguities between absolute and relative derivatives, the easiest way to obtain the EoM in body form is to transform their spatial expression. Since the physical quantities we are considering live in se∗(3), a relation is known to transform their represen-tation from one reference frame into another, i.e. Eq. (2.44). In the particu-lar case we are considering, we want to transform the spatial expression of our quantities into their body representations, so the transformation to be applied is Ad∗gbs = Ad−>gbs = Ad>gsb. Let us apply such transformation to both the members of Eq. (2.65), such that
Ad>gsbhMsV˙sbs + ad∗Vs sbM sVs sb i = Ad>gsbws Ms can be transformed using formula (2.48), obtaining
Ad>g sb h Ad−>g sb M bAd−1 gsb ˙ Vs sb+ ad ∗ Vs sbAd −> gsb M bAd−1 gsbV s sb i = Ad>g sbw s
This expression can be simplified, so as to obtain MbV˙sbb + ad∗Vb
sbM
bVb
This is the body form of the EoM. In an expanded block-matrix form, Eq. (2.72) may also be written as follows
" mI3×3 −m ˆdbg m ˆdbg JbOb # ˙vb ˙ωb + ˆ ωb 0 3×3 ˆ vb ωˆb m(vb+ ˆωbd bg) JbObω b+ m ˆd bgvb = Fb Mb Ob (2.73) In the following paragraphs, the terms of this equation will be analyzed in detail, separating the first three scalar equations from the last three.
Newton’s second law
Let us show how the first three components of the EoM written in their body form, i.e. Eq. (2.72), are equivalent to Newton’s second law. From the calculation of the upper blocks of Eq. (2.73), the following 3D-vector equation is obtained
m ˙vb− m ˆdbg˙ωb+ ˆωbm(vb+ ˆωbdbg) = Fb By substituting ˙vb with its expression (2.38), we get
mhR>sbd¨sb− ˆωbvb i
− m ˆdbg˙ωb+ ˆωbm(vb+ ˆωbdbg) = Fb Then, simplification yields
m R>sbd¨sb+ ˆ˙ωbdbg+ ˆ ωb2d bg = Fb Considering that R>
sbd¨sbis the absolute acceleration of point Obwith respect to the fixed point Os, expressed in{B} coordinates, and that dbg represents the position of G with respect to Ob, expressed in {B} coordinates, the term in the square brackets in this equation is the absolute acceleration aG of the centre of mass, expressed in{B} coordinates, too. This proves that the 3D-vector equation we are considering is equivalent to the body-fixed representation of Newton’s second law d dtQ b = mab G= Fb (2.74) Euler’s equations
The last three components of Eq. (2.72) represent the rotational dynamics of the rigid body, through Euler’s equations, written in body form. In fact, the lower block of Eq. (2.73) yields
m ˆdbg˙vb+ JbOb˙ω
b+ ˆvbm(vb+ ˆωbd
bg) + ˆωbJbObω
b+ mˆωbdˆ
bgvb = MObb
Substituting ˙vb with its expression (2.38) and rearranging the terms, we get m ˆdbgRsb>d¨sb+ JbOb˙ω b+ ˆωb JbObω b+ mhvˆbωˆbd bg + ˆωbdˆbgvb+ ˆdbgvˆbωb i = Mb Ob
2.6 Equations of Motion 31
The term inside the square brackets is null because of the Jacobi identity. More-over, as already remarked, R>sbd¨sb is the absolute acceleration aOb represented in
the reference frame{B}. This proves that the 3D-vector equation we are analyz-ing is equivalent to the body-fixed representation of Euler’s equations, written in the following form
d dt hrel Ob b + (ObG)b× mabOb = M b Ob (2.75)
