se3.m, so3.m, su2.m, kinematics.m,dynamics.m,
— Matematica packages for symbolic comutations on
matrix Lie groups in the context of rigid multibody
system dynamics —
Andreas Müller
Institute of Mechatronics e.V., Chemnitz, Germany
http://www.tu—chemnitz.de/ifm/english/projekte/ep_sing_dfg.htm
December, 2001 / May 2003
The packages are based on the paper
A. Müller, P. Maisser: „Lie group formulation of kinematics and dynamics of constrained MBS
and its application to analytical mechanics”, Multibody System Dynamics, Kluwer, 9, 2003,
pp. 311-352
According to the Mathematica notation arguments arg__ are optional. If not given the respective default value is used. $(MathematicaPath) denotes the directory where Mathematica is
installed.
All example notebooks use the style MbsTextbook. In order to make your notbook look nice
the corresponding style file
MBSTextbook.nb
should be copied in
$(MathematicaPath)/Configuration/FrontEnd/StyleSheets
1
2
Symbolic operations on the group SU (2)
Generalities
• Package name: su2.m
• Location: $(MathematicaPath)/AddOns/ExtraPackages/lie
Arguments are checked to be of the type specified.
Provided variables
Variable
Description
SU2Identity
Identity element in SU (2)
su2E1
First basis vector E1 of the standard basis {E1 , E2 , E3 } on su (2)
su2E2
Second basis vector E1 of the standard basis {E1 , E2 , E3 } on su (2)
su2E3
Third basis vector E1 of the standard basis {E1 , E2 , E3 } on su (2)
Transformation operators
• AxisTosu2[X]
Result
Argument
#1 X ∈ R3
rotation axis
• su2ToAxis[ω]
Argument
#1 ω ∈ so (3) skew matrix
b ∈ so (3)
ωX = X
¢
¡
b = −εijk X k
X
ωX - skew symmetric matrix
associated to the vektor X
Result
X = − 12 εijk ωjk
∨
X ≡ ω - vektor corresponding to the
skew symmetric matrix ω ∈ so (3)
3
Functions
• su2Exp[ω,θ ]
• XRotatationToSU2
• YRotatationToSU2
• ZRotatationToSU2
• SU2ToSO3
• su2Toso3
• SU2ToEulerParameter
• EulerParameterToSU2
• CayleyParameterToEulerParameter
• CayleyKleinParameterToEulerParameter
• GenCayleyParameterToEulerParameter
• SU2Ad
• SU2AdInverse
• VectorQuaternionTosu2
4
Symbolic operations on the three
dimensional rotation group SO (3)
Generalities
• Package name: so3.m
• Location: $(MathematicaPath)/AddOns/ExtraPackages/lie
Arguments are checked to be of the type specified.
Provided variables
Variable
Description
SO3Identity
Identity element in SO (3)
so3L1
First basis vector L1 of the standard basis {L1 , L2 , L3 } on so (3)
so3L2
Second basis vector L1 of the standard basis {L1 , L2 , L3 } on so (3)
so3L3
Third basis vector L1 of the standard basis {L1 , L2 , L3 } on so (3)
Transformation operators
• AxisToso3[X]
Result
Argument
#1 X ∈ R3
rotation axis
• so3ToAxis[ω]
Argument
#1 ω ∈ so (3) skew matrix
b ∈ so (3)
ωX = X
¢
¡
b = −εijk X k
X
ωX - skew symmetric matrix
associated to the vektor X
Result
X = − 12 εijk ωjk
∨
X ≡ ω - vektor corresponding to the
skew symmetric matrix ω ∈ so (3)
5
Functions
• so3Exp[ω,θ ]
Argument
Result
#1 ω ∈ so (3) skew matrix
#2 θ ∈ R
rotation angle
θ := 1 if not given
R = exp (ωθ)
R ∈ SO (3)
R - rotation matrix
describing rotation about
∨
axis ω and angle θ
so3Exp[X,θ ]
Argument
#1 X ∈ R3
#2 θ ∈ R
Result
rotation vector
rotation angle
θ := 1 if not given
R = exp (ωθ)
R ∈ SO (3)
R - rotation matrix
describing rotation about
axis X and angle θ
• SO3Ad[R,ω]
Argument
#1 R ∈ SO (3)
#2 ω ∈ so (3)
Result
η = AdR (ω)
≡ R · ω · RT
Adjoint action of SO (3) on so (3)
SO3Ad[R,X]
Argument
#1 R ∈ SO (3)
#2 X ∈ R3
Result
³ ´∨
b
Y = AdR X
≡R·X
Vektor representation of the
Adjoint action of SO (3) on so (3)
Remark: AdR (ωX )∨ also describes a rotation of X by R
• SO3AdInverse[R,ω]
Argument
#1 R ∈ SO (3)
#2 ω ∈ so (3)
Result
η = Ad−1
R (ω)
T
≡R ·ω·R
Adjoint action of inverse R−1
SO3AdInverse[R,X]
Argument
#1 R ∈ SO (3)
#2 X ∈ R3
• SO3ad[ω, η]
Result
³ ´∨
b
X
Y = Ad−1
R
≡ RT · X
Adjoint action of inverse R−1
in vector representation
6
Argument
#1 ω ∈ so (3)
#2 η ∈ so (3)
Result
ζ = adω (η) , ζ ∈ so (3)
≡ [ω, η] ≡ ω · η − η · ω,
adjoint action of so (3) on itself
i.e. the Lie bracket on so (3)
SO3ad[X,Y]
Argument
#1 X ∈ R3
#2 Y ∈ R3
Result
Z = adω (η)∨ , Z ∈ R3
≡X ×Y
Lie bracket on so (3)
in vector representation
• SO3ToRotationAxis[R]
Argument
Result
X ∈ R3 , ³Rotation
axis such that
´
b
R = exp θX for some angle θ
#1 R ∈ SO (3)
• SO3ToRotationAngle[R]
Argument
Result
#1 R ∈ SO (3)
θ ∈ R, Rotation
³ ´ angle such that
b for some axis X
R = exp θX
• XRotationToSO3[θ]
Argument
#1 θ ∈ R
Result
R = exp (θL1 ) , R ∈ SO (3)
Roation matrix corresponding to
a rotation about the X axis
• YRotationToSO3[θ]
Argument
#1 θ ∈ R
Result
R = exp (θL1 ) , R ∈ SO (3)
Roation matrix corresponding to
a rotation about the Y axis
• ZRotationToSO3[θ]
Argument
#1 θ ∈ R
Result
R = exp (θL1 ) , R ∈ SO (3)
• EulerParameterToSO3[p]
Roation matrix corresponding to
a rotation about the Z axis
7
Argument
Result
#1 p ∈ R4 , p = (p0 , p1 , p2 , p3 )
pi — Euler parameter
R ∈ SO (3)
Rotation matrix in terms of Euler parameter
• CayleyParameterToSO3[c]
Argument
Result
#1 c ∈ R3 , c = (c1 , c2 , c3 )
ci — Cayley parameter
R ∈ SO (3)
Rotation matrix in terms of Cayley parameter
Remark: Cayley parameter are also called Rodrigues parameter
• GenCayleyParameterToSO3[a]
Argument
Result
#1 a ∈ R3 , a = (a1 , a2 , a3 )
ai — generalized Cayley parameter
R ∈ SO (3)
Rotation matrix in terms of Cayley parameter
• AxisAngleToEulerParameter[X,θ]
Argument
#1 X ∈ R3
X — Rotation axis
#2 θ ∈ R
θ — Rotation angle
Result
p ∈ R4
Euler parameter corresponding
to a rotation about axis X and angle θ
• CayleyParameterToEulerParameter[c]
Argument
#1 c ∈ R3 , c = (c1 , c2 , c3 )
ci — Cayley parameter
Result
p ∈ R4
pi — corresponding Euler parameter
• GenCayleyParameterToEulerParameter[a]
Argument
#1 a ∈ R3 , a = (a1 , a2 , c3 )
ai — generalized Cayley parameter
Result
p ∈ R4
pi — corresponding Euler parameter
• SO3InvolutiveClosure[Ω]
Argument
#1 Ω = {ω1 , . . . , ωn } , ωi ∈ so (3)
Result
Involutive closure of the distribution
D = span (ω1 , . . . , ωn ) ⊆ so (3)
8
SO3InvolutiveClosure[W]
Argument
Result
#1 W = {X1 , . . . , Xn } , Xi ∈ R3
Miscellaneous functions
Involutive ³closure of the
´ distribution
b
b
D = span X1 , . . . , Xn ⊆ so (3)
• so3Q[ω]
Argument
#1 ω
Result
False
True
if w ∈
/ so (3)
if w ∈ so (3)
Purpose: Check if ω belongs to so (3)
• SO3Q[R]
Argument
#1 R
Result
False
True
if w ∈
/ SO (3)
if w ∈ SO (3)
Purpose: Check if R is a rotation matrix
9
Symbolic computations on the special
Euclidean group SE (3)
Generalities
• Package name: se3.m
• Location: $(MathematicaPath)/AddOns/ExtraPackages/lie
• S denotes the algebra of screw
• T denotes the algebra of twists
Provided variables
Variable
SE3Identity
Description
Identity element in SE (3)
Transformation operators
• AxisToScrew[ω,p]
Argument
#1 ω ∈ R3
#2 p ∈ R3
Result
screw axis
position vector of screw
• AxisTose3[ω,p]
Argument
#1 ω ∈ R3
#2 p ∈ R3
X = (ω, p × ω)
Plücker vector of screw
with axis ω trough p
Result
screw axis
position vector of screw
• ScrewTose3[X]
Argument
#1 X = (ω, v) ∈ S
ω, v ∈ R3
X = (ω, p × ω)∧
se (3) matrix associated to screw X
with parameter p and ω
Result
screw vector
b
X=X
se (3) matrix corresponding to screw vector X
10
• se3ToScrew[X]
Argument
Result
∨
#1 X ∈ se (3)
X =X
screw vector corresponding to se (3) matrix X
• se3ToV[X]
Argument
Result
∨
#1 X ∈ se (3)
v in X= (ω, v)
linear component of screw X
• se3ToOmega[X]
Argument
Result
∨
#1 X ∈ se (3)
ω in X= (ω, v)
angular component of screw X
• Twist[h__,ω,p]
Argument
#1 h ∈ R
#2 ω ∈ R3
#3 p ∈ R3
Result
pitch
screw axis
position vector of screw
X = (ω, p × ω + hω)
Twist vector of with axis ω
trough p having the pitch h
Remark: If h is not given its default value h = ∞ is used.
• TwistTose3[V]
Argument
#1 V ∈ T
Result
twist vector
V = Vb
se (3) matrix corresponding to twist vector V
Remark: The only difference between TwistTose3 and ScrewTose3 is the type checking.
• TwistToV[V]
Argument
#1 V ∈ T
Result
twist vector
∨
v in V= (ω, v)
linear velocity of twist vector X
Remark: The only difference between TwistToV and ScrewToV is the type checking.
• TwistToOmega[V]
Argument
#1 V ∈ T
twist vector
Result
∨
ω in V= (ω, v)
angular velocity of twist vector V
11
Remark: The only difference between TwistToOmega and ScrewToOmega is the type checking.
• Vector6ToHomogenousMatrix
Argument
#1 V ∈ R6
Result
Vb
se (3) matrix corresponding to V
considered as twist vector
Remark: Vector6ToHomogenousMatrix performs no type checking thus accepts any six vector.
• Vector6ToV[X]
Argument
#1 V ∈ R6
Result
∨
v in V = (ω, v)
linear part of V beeing considered as twist vector
Remark: Vector6ToV performs no type checking thus accepts any six vector.
• Vector6ToOmega
Argument
#1 V ∈ R6
Result
∨
ω in V = (ω, v)
angular part of V beeing considered as twist vector
Remark: Vector6ToOmega performs no type checking thus accepts any six vector.
• HomogenousMatrixToVector6[A]
Argument
µ
¶
ω v
#1 A =
∈ R4,4
0 0
ω ∈ so (3) , v ∈ R3
Result
³∨ ´
X = ω, v ∈ R6
Vector of homogenous coordinates
of homogenous velocity matrix A
Remark: HomogenousMatrixToVector6 performs no type checking.
• HomogenousMatrixToV[A]
Argument
#1 A ∈ R4,4
Result
v ∈ R3
Vector of homogenous coordinates
of homogenous velocity matrix A
Remark: HomogenousMatrixToV performs no type checking.
12
• RPToSE3[R,p]
Argument
Result
#1 R ∈ SO (3) Rotation matrix
#2 p ∈ R3
Position vecor
• RPToHomogenous[R,p]
Argument
R p
0 1
¶
∈ SE (3)
R p
0 1
¶
∈ SE (3)
Result
#1 R ∈ SO (3) Rotation matrix
#2 p ∈ R3
C=
µ
C=
Position vecor
µ
Remark: RPToHomogenous performs no type checking. This the only difference to RPToSE3.
• SE3ToSO3[C]
Argument
#1 C ∈ SE (3)
• SE3ToT3[C]
Argument
#1 C ∈ SE (3)
• HomogenousToSO3[C]
Argument
#1 C ∈ SE (3)
Result
R ∈ SO (3)
Rotation matrix in C
Result
p ∈ R3
Position vector in C
Result
p ∈ SO (3)
Rotation matrix in C
Remark: HomogenousToSO3 performs no type checking. This the only difference to SE3ToSO3.
• HomogenousToT3[C]
Argument
#1 C ∈ SE (3)
Result
p ∈ R3
Position vector in C
Remark: HomogenousToT3 performs no type checking. This the only difference to SE3ToT3.
13
Functions
• SE3Exp[X,t ]
Argument
#1 X∈ se (3)
#2 t ∈ R
Result
C = exp (tX) - Exponential of tX
angle
Remark: If t is not given it is set to t = 1.
• SE3ExpAxisPos[ω, r, t, h ]
Argument
#1 ω ∈ R3
#2 r ∈ R3
#3 t ∈ R
screw axis
position of axis
angle
Result
C = exp (tX) ≡
µ
R (I − R) r + hω
0
1
¶
,
b ≡ (ω, r × ω + hω)
with X≡ X
The rotation matrix R (t) = etωb describes the rotation about the axis ω and angle t.
• SE3ExpAxisPosEP[ω, r, t, h ]
Argument
#1 ω ∈ R3
#2 r ∈ R3
#3 p = {p0 , p1 , p2 , p3 } ∈ R4
screw axis
position of axis
Euler-parameter
Result
C = exp (tX) ≡
µ
R (I − R) r
0
1
¶
,
b ≡ (ω, r × ω + hω)
with X≡ X
The rotation matrix R (p) is determined by the Euler-parameter {p0 , p1 , p2 , p3 }.
The Euler-parameter have to be chosen so that R (p) describes the rotation
about the axis ω.
Note: The pitch is ignored, i.e. only pure rotations or translations are possible
• LogSE3Vector[C]
Argument
#1 C ∈ SE (3)
Result
X such that C = exp (tX)
14
• SE3Inverse[C]
Argument
#1 C ∈ SE (3)
• SE3Conjugation[C,H]
Argument
#1 C ∈ SE (3)
#2 H ∈ SE93
• SE3Ad[C,X]
Argument
#1 C ∈ SE (3)
#2 X ∈ se (3)
SE3Ad[C,X]
Argument
#1 C ∈ SE (3)
#2 X ∈ T
• SE3AdMatrix[C]
Argument
#1 C ∈ SE (3)
Result
C −1 - Inverse of C
Result
C H C −1
Conjugation of H by C
Result
AdC (X) ≡ CXC −1
Adjoined action of C on X ∈ se (3)
Result
AdC (X)∨ ≡ AdC · X
Adjoined action of C on twist X
Result
AdC ∈ R6,6
Matrix representation of Adjoined action, such that
∨
AdC · X ≡ AdC (X)∨ , for X =X
• SE3AdInverse
equivalent to SE3Ad[C,X] but with C −1
• SE3AdInverseMatrix
equivalent to SE3Ad[C] but with C −1
• SE3ad[X,Y]
Argument
#1 X ∈ T
#2 Y ∈ T
• SE3CoAdMatrix
Argument
#1 C ∈ SE (3)
#2
Result
Lie bracket of the two screw entities X and Y
[X, Y] ≡ adX (Y) ≡ XY − YX ∈ se (3)
Result
AdC ∈ R6,6
Matrix representation of the coadjoined action, such that
∨
AdTC X ≡ Ad∗C (X)∨ , for X =X
15
• se3Normalize[X]
Argument
#1 X ∈ T
Result
Normalize the component vectors in X ≡ (ω, v),
such that kωk = 1 and kvk = 1
• SE3InvolutiveClosure[D]
Argument
Result
#1 D = {X1 , . . . , Xn } , Xi ∈ se (3)
Involutive closure of the distribution
D = span (X1 , . . . , Xn ) ⊆ se (3)
• SE3InvolutiveClosure[D]
Argument
Result
#1 D = {X1 , . . . , Xn } , Xi ∈ S
Involutive closure of the distribution
D = span (X1 , . . . , Xn ) ⊆ S
• SE3InvolutiveClosureProjector
• SE3DistributionProjector
Miscellaneous functions
• SE3Identity
gives the identity element in SE (3)
• se3Q[X]
Argument
#1 X
Result
False
True
if X ∈
/ se (3)
if X ∈ se (3)
Purpose: Check if X belongs to se3
• ScrewQ[X]
Argument
#1 X
Result
False
True
Purpose: Check if X belongs to S
• TwistQ
if X ∈
/S
if X ∈ S
16
Argument
#1 X
Result
False
True
if X ∈
/T
if X ∈ T
Purpose: Check if X belongs to T
• SE3Q
Argument
#1 X
Result
False
True
if X ∈
/ SE (3)
if X ∈ SE (3)
Purpose: Check if X belongs to SE (3)
17
Kinematics of rigid multibody systems
- Symbolic and algebraic computation Generalities
• Package name: kinematics.m
• Location: $(MathematicaPath)/AddOns/ExtraPackages/lie
Basic functions for the kinematics of tree structured rigid MBS
The functions bellow assume arguments of the following form
L = {L (1),...,L (n)}
ConfList = {{M, X, q1 },...,{M, X, q n }}
1 1
n n
ConfListEP = {{M, ω , p,{p01 , p11 , p21 , p31 }},...,{M, ω , p,{p0n , p1n , p2n , p3n }}}
1 1 1
ConfListPOE = {{m, Y, q1 },...,{m, Y, qn }}
1
n
1
n n n
n
Here L (i) gives the index of the predecessor body of body i. The matrix M is the constant
i
transformation from body i to its predecessor L (i), X is the coresponding joint screw axis and
i
qi is the variable of joint i connecting the two bodies. If the POE formula is used m≡C (0) is
i
i
the reference configuration in the reference configuration q = 0 and Y is the joint screw w.r.t.
i
the ground fixed inertial frame in this reference configuration. In case of Euler-parameter
description {p0i , p1i , p2i , p3i } are the Euler-parameter describing the rotation of joint i.
• Config[M,X,q]
Argument
#1 M ∈ SE (3)
#2 X ∈ se (3)
#3 q ∈ R
• ConfigPOE[m,Y,q]
Result
C = MeXq - configuration of a reference frame
w.r.t. a chosen reference frame determined by the
constant configuration M and the screw motion
given by eXq , where q is the screw angle.
18
Argument
#1 m ∈ SE (3)
#2 Y ∈ se (3)
#3 q ∈ R
Result
C = eYq m - configuration of a reference frame
w.r.t. a chosen reference frame determined by the
reference configuration m ≡ C (0) and the screw motion
given by eYq , where q is the screw angle.
• ConfigEP[M,ω, r,p]
#1
#2
#3
#4
Argument
Result
M ∈ SE (3)
ω ∈ R3
r ∈ R3
p = {p0 , p1 , p2 , p3 } ∈ R4
C = MR (p) - configuration of a reference frame
w.r.t. a chosen reference frame determined by the
constant configuration M and R (p) determined
by the axis and position of the screw. The
rotation is found from the Euler-parameter p
screw axis
axis pos.
Eulerparameter
Note: The Euler-parameter description is only applicable to revolute joints and assemblies
thereof, like universal and spherical joints.
• RelativeConfiguration[ConfList, L, i,j]
Argument
#1 ConfList
#2 L
#3 i
#4 j
Result
R=C −1 C - Relative configuration of body i
i,j
j
i
w.r.t. body j, when C and C are determined
i
first body)
(second body)
j
with body fixed reference frames.
• RelativeConfigurationPOE[ConfListPOE, L, i, j]
Argument
#1 ConfListPOE
#2 L
#3 i
#4 j
Result
R=C −1 C - Relative configuration of body i
i,j
j
i
w.r.t. body j, when C and C are determined
i
(first body)
(second body)
j
without body fixed reference frames.
• RelativeConfigurationEP[ConfListEP, L, i, j]
Argument
#1 ConfListEP
#2 L
#3 i
#4 j
Result
R=C −1 C - Relative configuration of body i
i,j
j
i
w.r.t. body j, when C and C are determined
i
(body number)
(second body)
j
with body fixed reference frames.
R=R (p0i , p1i , p2i , p3i ) is given in terms of Euler-parameter
i,j
i,j
19
Note: The Euler-parameter description is only applicable to revolute joints and assemblies
thereof, like universal and spherical joints.
• Configuration[ConfList, L, i]
Argument
Result
Xq l
Xq i
#1 ConfList
C =M e l . . . M e i - Configuration of body i w.r.t.
#2 L
#3 i
the inertal frame found with body fixed reference frames
(l = min L - index of joint connecting body i with ground)
(body number)
i
i
l
• ConfigurationEP[ConfList, L, i]
Argument
Result
#1 ConfListEP
C =R · · · R - Configuration of body i w.r.t.
#2 L
#3 i
the inertal frame found with body fixed reference frames.
(l = min L - index of joint connecting body i with ground)
The configurations are given in terms of Euler-parameter
(body number)
i
0,l
L(i),i
Note: The Euler-parameter description is only applicable to revolut joints and assemblies
thereof, like universal and spherical joints.
• ConfigurationPOE[ConfListPOE, L, i]
Argument
Result
Yq l
Yq i
m - Configuration of body i w.r.t.
#1 ConfListPOE
C= e l . . . e i
#2 L
#3 i
the inertal frame found without body fixed reference frames
(l = min L - index of joint connecting body i with ground)
(body number)
i
i
• ConfigurationList[ConfList, L]
Argument
#1 ConfListPOE
#2 L
Result
List of configurations of all bodies of the MBS
using body fixed reference frames
• ConfigurationListPOE[ConfListPOE, L]
Argument
#1 ConfListPOE
#2 L
Result
List of configurations of all bodies of the MBS
using one space fixed reference frames
20
• ConfigurationListEP[ConfListEP, L]
Argument
#1 ConfListPOE
#2 L
Result
List of configurations of all bodies of the MBS
using body fixed reference frames
and Euler-parameter description
• KBF1[ConfList, L, i]
Argument
Result
#1 ConfList
K - kinematic basic function (KBF) of the first kind
#2 L
#3 i
of body i, when body fixed reference frames are used.
The KBF of first kind yields the body velocity
of body i via V=K a q̇ a
(body number)
i
i
i
• KBF1POE[ConfListPOE, L, i]
Argument
Result
#1 ConfListPOE
K - kinematic basic function (KBF) of the first kind
#2 L
#3 i
of body i, when no body fixed reference frames are used.
i
(body number)
• KBF2[ConfList, L, i]
Argument
Result
s
#1 ConfList
K - kinematic basic function (KBF) of the second kind
#2 L
#3 i
of body i, when body fixed reference frames are used.
The KBF of second kind yields the spacial velocity
(body number)
i
s
s
i
i
of body i via V=K a q̇ a
• KBF2POE[ConfListPOE, L, i]
Argument
Result
s
#1 ConfListPOE
K - kinematic basic function (KBF) of the second kind
#2 L
#3 i
of body i, when no body fixed reference frames are used.
(body number)
i
21
• KBF1Matrix[ConfList, L]
Argument
Result
#1 ConfList
#2 L
Matrix composed of the KBF matrices of first kind
of all bodies in the MBS.
• KBF2Matrix[ConfList, L]
Argument
Result
#1 ConfList
#2 L
Matrix composed of the KBF matrices of second kind
of all bodies in the MBS.
Functions for the kinematics of MBS with kinematic loops
For each kinematic loop a cut joint ji,j is chosen and cut open that connects body i and j. The
joint variable q i,j of this cut joint is the cut joint variable, which is not a generalized coordinate.
The geometric loop closure conditions are expressed using the cut joint reference frame (CJFR)
on either body. The configuration of the CJFR on body i and j is M and M respectively. The
i,j
j,i
screw coordinates X of the cut joint ji,j are expressed in the CJFR on body j. The arguments
i,j
are presumed to be of the following form
cutL = {{i, j},...}
cutConfigList = {{M , X , qi,j },...}
i,j i,j
cutL is a list of indices of the body pairs connected by cut joints. The list cutConfigList
containes one entry for each cut joint describing the cut joint geometry as outlined above.
• GeometricLoopConstraints[ConfigList,L,cutL,cutConfigList]
#1
#2
#3
#4
Argument
Result
ConfigList
L
cutL
cutConfigList
List of geometric closure conditions,
one for each cut joint
• KinematicLoopConstraints[ConfigList,L,cutL,cutConfigList]
#1
#2
#3
#4
Argument
Result
ConfigList
L
cutL
cutConfigList
List of kinematic closure conditions,
one for each cut joint
22
• ReduceKinematicLoopConstraints[ConfigList,L,cutL,cutConfigList]
#1
#2
#3
#4
Argument
Result
ConfigList
L
cutL
cutConfigList
List of reduced kinematic closure conditions,
of which possibly dependent constraint equations
are eliminated.
Note: This reduction is very rudimentary.
• KinematicLoopConstraintsRoot[ConfigList,L,cutL,cutConfigList]
#1
#2
#3
#4
Argument
Result
ConfigList
L
cutL
cutConfigList
List of kinematic closure conditions,
one for each cut joint. The constraints are
expressed in the root body of the loop.
• ReduceKinematicLoopConstraintsRoot[ConfigList,L,cutL,cutConfigList]
#1
#2
#3
#4
Argument
Result
ConfigList
L
cutL
cutConfigList
List of reduced kinematic closure conditions,
of which possibly dependent constraint equations
are eliminated.
Note: This reduction is very rudimentary.
Miscellaneous functions
• BranchRoot[L,i,j]
Returns the index of the body at which the path from body i and body j to the ground
(body 0) meet, when L describes a tree structured MBS.
• isPredecessor[L,k,i]
Returns True if body i is predecessor of body k and False otherwise.
23
Dynamics of rigid multibody systems
- Symbolic and algebraic computation Generalities
• Package name: dynamics.m
• Location: $(MathematicaPath)/AddOns/ExtraPackages/lie
Functions
The functions bellow assume arguments of the following form
COMList
= {{ M , , ..., M }
1
n
InertListCOM = {{m1 , Ixx,1 , Iyy,1 , Izz,1 },...,{mn , Ixx,n , Iyy,n , Izz,n }}
InertListRFR = {J , . . . , J }
1
n
Here COMList holds the transformations from the reference frames to the COM frame. InertListCOM
contains for each body i its mass mi and the principal inertia moments Ixx,i , Iyy,i , Izz,i (the COM
frame is assumed to be alligned with the principal axis of inertia). the index of the predecessor
body of body i. InertListRFR is the list of inertia tensors J for all bodies.
i
• InertiaListRFR[InertListCOM, COMList]
Argument
Result
#1 InertListCOM
#2 COMList
InertListRFR - List of inertia tensors for
w.r.t. the reference frame all bodies
• InertiaList[InertiaData]
Argument
#1 InertiaData = {{m1 , Jxx,1 , Jyy,1 , Jzz,1 , Jxy,1 , Jxz,1 , Jyz,1 , M },...
1
...,{mn , Jxx,n , Jyy,n , Jzz,n , Jxy,n , Jxz,n , Jyz,n , M }}
n
mk is the mass of bosy k, Jij,k are the inertia moments
w.r.t. to an arbitrary reference frame,
M is the transformation to another RFR
k
24
Result
Inertia tensors transformed in another RFR. The transformation from
one RFR to the another one on body k is M .
k
• Metric[ConfigList, L, InertListRFR]
Argument
#1 ConfigList
#2 L
#3 InertListRFR
Result
Generalized mass matrix (gab ) of the MBS,
i.e. the metric on the configuration space
induced by the kinetic energy
• ChristoffelSymbolsFirstKind[ConfigList, L, InertListRFR, i]
#1
#2
#3
#4
Argument
Result
ConfigList
L
InertListRFR
i (body number)
Γabc Christoffel-symbols 1. kind corresponding
to the metric gab . They are not found via
differentiation but algebraically.
• GeneralizedGravityForces[ConfigList, L, COMList, InertListRFR, g]
#1
#2
#3
#4
#5
Argument
Result
ConfigList
L
COMList
InertListRFR
g - vector of gravity
Generalized forces due to gravitation.
25
Utility funtions
Generalities
• Package name: utilities.m
• Location: $(MathematicaPath)/AddOns/ExtraPackages/lie
Functions
• Vector3Q[v]
Argument
#1 v
Result
False
True
if dim v =
6 3
if dim v = 3
Purpose: Check if v is some three dimensional vector
• Vector4Q[V]
Argument
#1 v
Result
False
True
if dim v =
6 4
if dim v = 4
Purpose: Check if v is some four dimensional vector
• RemoveZeroRows[M]
Argument
#1 M
Result
Removes all rows that only
contain zeros
• MakeBin[V]
Argument
#1 V
Result
Converts all nonzero elements in the vector V to 1
26
• MakeBin[M]
Argument
#1 M
Result
Converts all nonzero elements in the matrix M to 1
27
Summary of variables and functions
SO (3)
Provided variables
— SO3Identity
— so3L1
— so3L2
— so3L3
Provided functions
— AxisToso3
— so3ToAxis
— so3Exp
— SO3Ad
— SO3AdInverse
— SO3ad
— SO3ToRotationAxis
— SO3ToRotationAngle
— XRotationToSO3
— YRotationToSO3
— ZRotationToSO3
— EulerParameterToSO3
— CayleyParameterToSO3
— GenCayleyParameterToSO3
— AxisAngleToEulerParameter
— CayleyParameterToEulerParameter
— GenCayleyParameterToEulerParameter
— SO3InvolutiveClosure
— so3Q
— SO3Q
SU (2)
Provided variables
— Su2Identity
— su2E1
— su2E2
— su2E3
28
Provided functions
— su2Exp
— XRotatationToSU2
— YRotatationToSU2
— ZRotatationToSU2
— SU2ToSO3
— su2Toso3
— SU2ToEulerParameter
— EulerParameterToSU2
— CayleyParameterToEulerParameter
— CayleyKleinParameterToEulerParameter
— GenCayleyParameterToEulerParameter
— SU2Ad
— SU2AdInverse
— VectorQuaternionTosu2
SE (3)
Provided variables
— SE3Identity
Provided functions
— AxisToScrew
— AxisTose3
— ScrewTose3
— se3ToScrew
— se3ToV
— se3ToOmega
— Twist
— TwistTose3
— TwistToV
— TwistToOmega
— Vector6ToHomogenousMatrix
— Vector6ToV
— Vector6ToOmega
— HomogenousMatrixToVector6
— HomogenousMatrixToV
29
— RPToSE3
— RPToHomogenous
— SE3ToSO3
— SE3ToT3
— HomogenousToSO3
— HomogenousToT3
— SE3Exp
— SE3ExpAxisPos
— SE3ExpAxisPosEP
— LogSE3Vector
— SE3Inverse
— SE3Conjugation
— SE3Ad
— SE3AdMatrix
— SE3AdInverse
— SE3AdInverseMatrix
— SE3ad
— SE3CoAdMatrix
— se3Normalize
— SE3InvolutiveClosure
— SE3DistributionProjector
— SE3InvolutiveClosureProjector
— se3Q
— SE3Q
— ScrewQ
— TwistQ
Kinematics
Provided functions
— Config
— ConfigEP
— ConfigPOE
— RelativeConfiguration
— RelativeConfigurationPOE
— RelativeConfigurationEP
— Configuration
30
— ConfigurationEP
— ConfigurationPOE
— ConfigurationList
— ConfigurationListPOE
— ConfigurationListEP
— KBF1
— KBF1POE
— KBF2
— KBF2POE
— KBF1Matrix
— KBF2Matrix
— GeometricLoopConstraints
— KinematicLoopConstraints
— ReduceKinematicLoopConstraints
— KinematicLoopConstraintsRoot
— ReduceKinematicLoopConstraintsRoot
— BranchRoot
— isPredecessor
Dynamics
Provided functions
— InertiaListRFR
— InertiaList
— Metric
— ChristoffelSymbolsFirstKind
— GeneralizedGravityForces