The Geometric Framework
Stanislao Grazioso
Friday 6th April, 2018
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 1 / 43
Introduction
Screw theory
Differential geometry
Finite element method
This course will combine these three techniques/methods
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 2 / 43
Main aspects of a geometric framework
1. Rigid body transformationsCoupling of position and rotation variables
Kinematics using Euclidean transformations
Space of Euclidean transformations = Lie group structure
2. Lie derivativesDerivatives (deformations and velocities) and kinematic joint transformations expressed in local framesattached to the bodies.
Resulting EoM are invariant with respect to a superimposed Euclidean transformation, i.e. EoM do notdepend on the position and orientation of the bodies with respect to the inertial reference frame →reduced non−linearities in EoM
3. Global parametrization−free frameworkLie Group motion formalism avoids the parametrization of rotation variables → reduced non−linearitiesand no singularities
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 3 / 43
1. Rigid Body Transformations
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 4 / 43
Rigid body transformations
I
RRp
u
q = g(p)
p
reference configuration
current configuration
o
Definition
A mapping g : R3 → R3 is a rigid body transformation if:
Distance is preserved, i.e., ||g(pj)− g(pi )|| = ||pj − pi || ∀ pi ,pj ∈ R3
Cross product is preserved, i.e., u(a× b) = g(a)× g(b) ∈ R3
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 5 / 43
Rigid body transformations (cont’d)
I
RRp
u
q = g(p)
p
reference configuration
current configuration
o
Proposition
Rigid body transformations are such that
q = u + Rp (1)
where u ∈ R3 and R ∈ SO(3) is a rotation matrix, which satisfies
RTR = I3×3 det(R) = +1
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 6 / 43
Rigid body transformations (cont’d)
I
RRp
u
q = g(p)
p
reference configuration
current configuration
o
Equation 1 in matrix form↓[
q1
]= H(R,u)
[p1
]
H = H(R,u) =
[R u
01×3 1
]∈ SE (3)
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 7 / 43
Rigid body transformations (cont’d)
Change of reference frame = Euclidean transformation
The space SE (3) of Euclidean transformations is a Lie Group
H = H(R, x) =
[R x01x3 1
]∈ SE(3)
SE(3) = SO(3)× R3
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 8 / 43
Lie group
Definition
A group (G,·) is a set G of elements q together with a composition operation (·)which satisfies the four axioms of:
closure: the composition of two elements of the set yields an element of theset, i.e., ∀q1, q2 ∈ G , q1 · q2 = q3 ∈ G
associativity: q1 · (q2 · q3) = (q1 · q2) · q3
neutral element: there exists an element e of the set such thatq · e = e · q = q
inverse element: there exists an element q−1 of the set such thatq · q−1 = q−1 · q = e
DefinitionA Lie group is a continuous group for which the composition rule and the inverseare smooth
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 9 / 43
Lie group (cont’d)
Proposition
A matrix Lie group is a Lie group for which the composition rule is represented bythe matrix product
R ∈ SO(3), the special Orthogonal group
H ∈ SE (3), the special Euclidean group
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 10 / 43
2. Lie Derivatives
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 11 / 43
Lie derivatives
Derivative of a Lie group
The derivative of q ∈ G with respect to a ∈ R reads
da(q) = qaL
= aRq
where aL ∈ g and aR ∈ g are respectively called a left and right invariant vectorfield. These elements represent the Lie algebra associated to the Lie group.
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 12 / 43
Lie algebra
Definition
The Lie algebra g (se(3)) is the tangent space at the identity element of a Liegroup G (H).
da(H) = Ha
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 13 / 43
Lie algebra (cont’d)
Proposition
The Lie algebra g is isomorphic to Rk through the invertible linear map
(·) : Rk → g, a ∈ Rk 7→ a ∈ g
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 14 / 43
Lie algebra (cont’d)
SO(3) so(3) R3
SE (3) se(3) R6
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 15 / 43
Lie algebra (cont’d)
da(H) = Ha
Left invariant vector field on SE (3)=
Invariant under a superimposed Euclidean transformation=
Intrinsic quantity
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 16 / 43
Twist
Time derivative of a Lie group
The time derivative of H reads
H = HηL
= ηRH
where ηL ∈ se(3) and ηR ∈ se(3) are respectively called a left and right invariantvector field.
The element η ∈ se is the Lie algebra associated to the Lie group H ∈ SE (3).
In the screw theory , the Lie algebra η ∈ se is called twist.
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 17 / 43
Twist (cont’d)
se(3) and so(3)
The Lie algebra η is the 4× 4 matrix
η =
[ω v
01×3 1
]∈ se(3)
where the Lie algebra ω is the skew-symmetric matrix
ω =
0 −ω3 ω2
ω3 0 −ω1
−ω2 ω1 0
∈ so(3)
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 18 / 43
Twist (cont’d)
Velocity vector
According to the isomorphism g ' Rk , so(3) is isomorphic to R3 with
ω = [ω1 ω2 ω3]T , while se(3) is isomorphic to R6 with
η =
[v
ω
]
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 19 / 43
Screws
s
θq
pqθ
−sθ × q
Figure: A screw axis S represented by a point q, a unit direction vector s and a pitch p.
A geometrical interpretation of twist
The twist η corresponding to an angular velocity θ about the screw axis S:
η =
[v
ω
]=
[−s θ × q + ps θ
s θ
]Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 20 / 43
Wrenches
Force vectorSix–dimensional vector comprising the linear force and the moment as
τ =
[f
m
]∈ R6
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 21 / 43
Adjoint representation
DefinitionThe adjoint representation of a Lie algebra element is defined as
Adq : g→ g, a 7→ qaq−1
Adjoint representation of a se(3) element
AdH(a) = HaH−1
AdH(a) =
[R uR
03×3 R
]a
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 22 / 43
Lie bracket
DefinitionThe Lie bracket operator is the bilinear operator defined as
[·, ·] : g× g→ g,[a, b]7→ db(a)− da(b)
Cross derivatives
db(a)− da(b) =[a, b]
db(a)− da(b) = ab = adab a =
[aω aω03×3 aω
]
Definition
The linear operator (·) is the bilinear operator defined as
(·) : Rk → Rk×k , a 7→ a = A
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 23 / 43
Equations of motion of a rigid body
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 24 / 43
Variations
Variations of a Lie group element
δ(R) = Rδθ
δ(H) = Hδh
where δθ ∈ so(3) is an arbitrary infinitesimal rotation associated with the axialvector δθ ∈ R3 and δhu = RT δu ∈ R3 is an arbitrary infinitesimal displacement
Variations of a twist element
δ(η)− (δh)· =[η, δh
]δ(η)− (δh)· = ηδh = −δhη
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 25 / 43
Kinetic energy K
K =1
2
∫V
ρqT q dV
q = u + Rp
q = u + Rωp = R [I3×3 − p]η
⇓
K =1
2ηTMη
M =
[mI3×3 JT1 (p)J1(p) J2(p)
]
m =
∫V
ρ dV ; J1(p) =
∫V
ρp dV ; J2(p) =
∫V
ρpT p dV
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 26 / 43
Potential energy Vext
Vext =
∫V
qTge dV
ge = 3× 1 vector of applied external forces expressed in the fixed reference frame.
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 27 / 43
Hamiltonian formulation
Hamilton’s principle ∫ t1
t0
(δ(K)− δ(Vext)) dt = 0 .
δ(K) = δ(η)TMη =
= ((δhT )· + δhT ηT )Mη
δ(Vext) =
∫V
δ(q)Tge dV =
= δhTgext
δ(q) = R [I3×3 − p] δh
gext =
[gext,ugext,ω
]=
∫V
[I3×3
p
]RTge dV
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 28 / 43
Hamiltonian formulation
Dynamic equilibrium equations
weak form[δhT (Mη)
]t1
t0−∫ t1
t0δhT (Mη − ηTMη − gext) dt = 0
strong form Mη − ηTMη = gext
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 29 / 43
Hamiltonian formulation
Equations of motion of a rigid body
H = Hη
Mη − ηTMη = gext
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 30 / 43
3. Global parametrization–free framework
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 31 / 43
Global parametrization of rotation
Euler’s equations (free rotating rigid body)
R = Rω
Jω + ωTJω = 03x1
⇓ Global parametrization of rotation
R = R(α1, α2, α3)
ω = T(α)α
ω = T(α)α+ T(α, α)α
Discretized Euler’s equationsR = R(α)
(J(Tα+ Tα) + (αTTT )JTα) = 03x1
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 32 / 43
Global parametrization–free equations of motion
Euler’s equations (free rotating rigid body)
R = Rω
Jω + ωTJω = 03x1
⇓ Exponential map
R = Rω ⇒ R(t) = R0 expSO(3)(ωt)
Euler’s equations discretized on the Lie groupRn+1 = Rn expSO(3)(nn+1)
Jωn+1 + ωTn+1Jωn+1 = 03x1
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 33 / 43
Global parametrization–free equations of motion (cont’d)
Integration formulae (implicit generalized−α scheme)
nn+1 = hωn + (0.5− β)h2an + βh2an+1
ωn+1 = ωn + (1− γ)han + γhan+1
an+1 =1
(1− αm)((1− αf )ωn+1 + αf ωn − αman)
αm =2ρ− 1
ρ+ 1; αf =
ρ
ρ+ 1; γ =
3− ρ2(ρ+ 1)
; β =1
(ρ+ 1)2
ρ ∈ [0, 1]
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 34 / 43
Global parametrization–free equations of motion (cont’d)
Classic vs Geometric
Discrete dynamics (classic) TT (J(Tα+ Tα) + (Tα)JTα) = 03x1
Discrete dynamics (geometric) Jω + ωTJω = 03x1
non singularities singularitiesquadratic high nonlinearitiesintrinsic orientation dependent
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 35 / 43
Exponential map
DefinitionThe exponential map projects an element of the Lie algebra into an element of theLie group
exp : g→ G , a 7→ exp(a)
and it is given by
exp(a) =∞∑i=0
ai
i !
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 36 / 43
Logarithmic map
DefinitionThe logarithmic map projects an element of the Lie group into an element of theLie algebra
log : G → g, q 7→ log(q) = a
and it is given by
log(q) =∞∑i=0
(e − q)i
i
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 37 / 43
Tangent map
Definition
T : Rk → Rk , u 7→ T(u)da(u) = a
with
T(u) =∞∑i=0
(−1)iui
(i + 1)!
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 38 / 43
Inverse of the tangent map
Definition
T−1 : Rk → Rk , u, a 7→ T−1(u)a = da(u)
with
T−1(u) =∞∑i=0
(−1)iBiui
(i)!
where Bi is the Bernoulli number of the first kind.
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 39 / 43
The exponential map on SO(3)
Exponential map expSO(3)(hω) = I3×3 + α(hω)hω + β(hω)2 h2
ω
Logarithmic map logSO(3)(R) = θ2sinθ (R− RT )
Tangent operator TSO(3)(hω) = I3×3 − β(hω)2 hω + 1−α(hω)
‖hω‖2 h2ωs
Inverse of the tangent operator T−1SO(3)(hω) = I3×3 + 1
2 hω + 1−γ(hω)‖hω‖2 h2
ω
α(hω) =sin(‖hω‖)‖hω‖
β(hω) = 21− cos(‖hω‖)‖hω‖2
γ(hω) =‖hω‖
2cot
(‖hω‖
2
)
θ = acos
(1
2(trace(R)− 1
), θ < π
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 40 / 43
The exponential map on SE (3)
Exponential map expSE(3)(h) =
[expSO(3)(hω) TT
SO(3)(hω)hu01×3 1
]
Logarithmic map logSE(3)(H) =
[hω T−TSO(3)(hω)hu01×3 0
]
Tangent operator TSE(3)(h) =
[TSO(3)(hω) Tuω+(hu,hω)
03×3 TSO(3)(hω)
]
Inverse of the tangent operator T−1SE(3)(h) =
[T−1
SO(3)(hω) Tuω−(hu,hω)
03×3 T−1SO(3)(hω)
]
Tuω+(hω , hu) =−β2
hω +1− α‖hω‖2
[hω , hu ] +hTu hω
‖hu‖2
((β − α)hu + (
β
2−
3(1− α‖hu‖2
)h2u
)Tuω−(hω , hu) =
1
2hω +
1− γ‖hω‖2
[hω , hu ] +hTu hω
‖hu‖4
((
1
β+ γ − 2)h2
u
)Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 41 / 43
References (screw theory to multibody dynamics androbotics)
[Ball00] R Ball ”A treatise on the theory of screws”, 1900 (Reprinted 1998).
[Bro83] RW Brockett ”Robotic manipulators and the product of exponentials formula”,International symposium on the mathematical theory of networks and systems,pp. 120–129, 1983.
[MLS94] R M Murray, Z Li, S S Sastry, ”A mathematical introduction to roboticmanipulation”, CRC press, 1994.
[Se04] J M Selig ”Geometric fundamentals of robotics”, Springer Science andBusiness Media, 2004.
[LP17] K M Lynch and F C Park ”Modern robotics: Mechanics, Planning andControl”, Cambridge University Press, 2017.
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 42 / 43
References (geometric time integration)
[CG93] P E Crounch and R Grossman ”Numerical integration of ordinary differentialequations on manifolds”, Journal of Nonlinear Science, vol. 3, no. 1, pp. 1–33,1993.
[MK98] H Munthe–Kaas ”Runge-kutta methods on lie groups”, BIT NumericalMathematics, vol. 38, no. 1, pp. 92–111, 1998.
[MK98] J Park and W K Chung ”Geometric integration on Euclidean group withapplication to articulated multibody systems”, IEEE Transaction on Robotics,21(5), pp 850–863, 2005.
[BCA12] O Bruls, A Cardona and M Arnold ”Lie group generalized–α time integration ofconstrained flexible multibody systems”, Mechanism and Machine Theory, vol.48, no. 1, pp. 121–137, 2012.
[TMZ15] Z Terze, A Muller and D Zlatar ”Lie–group integration method for constrainedmultibody systems in state space”, Multibody System Dynamics, vol. 34, no.3, pp. 275–305, 2015.
Stanislao Grazioso Geometric Theory of Soft Robots Friday 6th April, 2018 43 / 43