Module: lieGroupBasics

Lie group methods and formulas for Lie group integration.

  • Author: Stefan Holzinger, Johannes Gerstmayr

  • Date: 2020-09-11

  • References: For details on Lie group methods used here, see the references [Henderson1977; Simo1988; Bruels2011; Sonneville2014; Sonneville2017; Terze2016; Mueller2017]. Lie group methods for rotation vector are described in Holzinger and Gerstmayr [HolzingerGerstmayr2020; Holzinger2021].

Function: Sinc

Sinc(x)

  • function description: compute the cardinal sine function in radians

  • input: scalar float or int value

  • output: float value in radians

  • author: Stefan Holzinger

Function: Cot

Cot(x)

  • function description: compute the cotangent function cot(x)=1/tan(x) in radians

  • input: scalar float or int value

  • output: float value in radians

  • author: Stefan Holzinger

Function: R3xSO3Matrix2RotationMatrix

R3xSO3Matrix2RotationMatrix(G)

  • function description: computes 3x3 rotation matrix from 7x7 R3xSO(3) matrix, see Bruels2011

  • input:

    • G: 7x7 matrix as np.array

  • output: 3x3 rotation matrix as np.array

  • author: Stefan Holzinger

Function: R3xSO3Matrix2Translation

R3xSO3Matrix2Translation(G)

  • function description: computes translation part of R3xSO(3) matrix, see Bruels2011

  • input:

    • G: 7x7 matrix as np.array

  • output: 3D vector as np.array containg translational part of R3xSO(3)

  • author: Stefan Holzinger

Function: R3xSO3Matrix

R3xSO3Matrix(x, R)

  • function description: builds 7x7 matrix as element of the Lie group R3xSO(3), see Bruels2011

  • input:

    • x: 3D vector as np.array representing the translation part corresponding to R3

    • R: 3x3 rotation matrix as np.array

  • output: 7x7 matrix as np.array

  • author: Stefan Holzinger

Function: ExpSO3

ExpSO3(Omega)

  • function description: compute the matrix exponential map on the Lie group SO(3), see Mueller2017

  • input: 3D rotation vector as np.array

  • output: 3x3 matrix as np.array

  • author: Stefan Holzinger

Relevant Examples (Ex) and TestModels (TM) with weblink to github: LieGroupIntegrationUnitTests.py (TM)

Function: ExpS3

ExpS3(Omega)

  • function description: compute the quaternion exponential map on the Lie group S(3), see [Terze2016; Mueller2017]

  • input: 3D rotation vector as np.array

  • output: 4D vector as np.array containing four Euler parameters entry zero of output represent the scalar part of Euler parameters

  • author: Stefan Holzinger

Function: LogSO3

LogSO3(R)

  • function description: compute the matrix logarithmic map on the Lie group SO(3)

  • input: 3x3 rotation matrix as np.array

  • output: 3x3 skew symmetric matrix as np.array

  • author: Johannes Gerstmayr

  • notes: improved accuracy for very small angles as well as angles phi close to pi AS WELL AS at phi=pi

Function: TExpSO3

TExpSO3(Omega)

  • function description: compute the tangent operator corresponding to ExpSO3, see Bruels2011

  • input: 3D rotation vector as np.array

  • output: 3x3 matrix as np.array

  • author: Stefan Holzinger

Function: TExpSO3Inv

TExpSO3Inv(Omega)

  • function description: compute the inverse of the tangent operator TExpSO3, see Sonneville2014 this function was improved, see coordinateMaps.pdf by Stefan Holzinger

  • input: 3D rotation vector as np.array

  • output: 3x3 matrix as np.array

  • author: Stefan Holzinger

Function: ExpSE3

ExpSE3(x)

  • function description: compute the matrix exponential map on the Lie group SE(3), see Bruels2011

  • input: 6D incremental motion vector as np.array

  • output: 4x4 homogeneous transformation matrix as np.array

  • author: Stefan Holzinger

Relevant Examples (Ex) and TestModels (TM) with weblink to github: serialRobotInverseKinematics.py (Ex)

Function: LogSE3

LogSE3(H)

  • function description: compute the matrix logarithm on the Lie group SE(3), see Sonneville2014

  • input: 4x4 homogeneous transformation matrix as np.array

  • output: 4x4 skew symmetric matrix as np.array

  • author: Stefan Holzinger

Relevant Examples (Ex) and TestModels (TM) with weblink to github: serialRobotInverseKinematics.py (Ex), homogeneousTransformationInterfaceTest.py (TM)

Function: TExpSE3

TExpSE3(x)

  • function description: compute the tangent operator corresponding to ExpSE3, see Bruels2011

  • input: 6D incremental motion vector as np.array

  • output: 6x6 matrix as np.array

  • author: Stefan Holzinger

  • notes: improved accuracy for very small angles as well as angles phi

Function: TExpSE3Inv

TExpSE3Inv(x)

  • function description: compute the inverse of tangent operator TExpSE3, see Sonneville2014

  • input: 6D incremental motion vector as np.array

  • output: 6x6 matrix as np.array

  • author: Stefan Holzinger

  • notes: improved accuracy for very small angles as well as angles phi

Function: ExpR3xSO3

ExpR3xSO3(x)

  • function description: compute the matrix exponential map on the Lie group R3xSO(3), see Bruels2011

  • input: 6D incremental motion vector as np.array

  • output: 7x7 matrix as np.array

  • author: Stefan Holzinger

Function: TExpR3xSO3

TExpR3xSO3(x)

  • function description: compute the tangent operator corresponding to ExpR3xSO3, see Bruels2011

  • input: 6D incremental motion vector as np.array

  • output: 6x6 matrix as np.array

  • author: Stefan Holzinger

Function: TExpR3xSO3Inv

TExpR3xSO3Inv(x)

  • function description: compute the inverse of tangent operator TExpR3xSO3

  • input: 6D incremental motion vector as np.array

  • output: 6x6 matrix as np.array

  • author: Stefan Holzinger

Function: CompositionRuleDirectProductR3AndS3

CompositionRuleDirectProductR3AndS3(q0, incrementalMotionVector)

  • function description: compute composition operation for pairs in the Lie group R3xS3

  • input:

    • q0: 7D vector as np.array containing position coordinates and Euler parameters

    • incrementalMotionVector: 6D incremental motion vector as np.array

  • output: 7D vector as np.array containing composed position coordinates and composed Euler parameters

  • author: Stefan Holzinger

Function: CompositionRuleSemiDirectProductR3AndS3

CompositionRuleSemiDirectProductR3AndS3(q0, incrementalMotionVector)

  • function description: compute composition operation for pairs in the Lie group R3 semiTimes S3 (corresponds to SE(3))

  • input:

    • q0: 7D vector as np.array containing position coordinates and Euler parameters

    • incrementalMotionVector: 6D incremental motion vector as np.array

  • output: 7D vector as np.array containing composed position coordinates and composed Euler parameters

  • author: Stefan Holzinger

Function: CompositionRuleDirectProductR3AndR3RotVec

CompositionRuleDirectProductR3AndR3RotVec(q0, incrementalMotionVector)

  • function description: compute composition operation for pairs in the group obtained from the direct product of R3 and R3, see HolzingerGerstmayr2020 the rotation vector is used as rotation parametrizations this composition operation can be used in formulations which represent the translational velocities in the global (inertial) frame

  • input:

    • q0: 6D vector as np.array containing position coordinates and rotation vector

    • incrementalMotionVector: 6D incremental motion vector as np.array

  • output: 7D vector as np.array containing composed position coordinates and composed rotation vector

  • author: Stefan Holzinger

Function: CompositionRuleSemiDirectProductR3AndR3RotVec

CompositionRuleSemiDirectProductR3AndR3RotVec(q0, incrementalMotionVector)

  • function description: compute composition operation for pairs in the group obtained from the direct product of R3 and R3. the rotation vector is used as rotation parametrizations this composition operation can be used in formulations which represent the translational velocities in the local (body-attached) frame

  • input:

    • q0: 6D vector as np.array containing position coordinates and rotation vector

    • incrementalMotionVector: 6D incremental motion vector as np.array

  • output: 6D vector as np.array containing composed position coordinates and composed rotation vector

  • author: Stefan Holzinger

Function: CompositionRuleDirectProductR3AndR3RotXYZAngles

CompositionRuleDirectProductR3AndR3RotXYZAngles(q0, incrementalMotionVector)

  • function description: compute composition operation for pairs in the group obtained from the direct product of R3 and R3. Cardan-Tait/Bryan (CTB) angles are used as rotation parametrizations this composition operation can be used in formulations which represent the translational velocities in the global (inertial) frame

  • input:

    • q0: 6D vector as np.array containing position coordinates and Cardan-Tait/Bryan angles

    • incrementalMotionVector: 6D incremental motion vector as np.array

  • output: 6D vector as np.array containing composed position coordinates and composed Cardan-Tait/Bryan angles

  • author: Stefan Holzinger

Function: CompositionRuleSemiDirectProductR3AndR3RotXYZAngles

CompositionRuleSemiDirectProductR3AndR3RotXYZAngles(q0, incrementalMotionVector)

  • function description: compute composition operation for pairs in the group obtained from the direct product of R3 and R3. Cardan-Tait/Bryan (CTB) angles are used as rotation parametrizations this composition operation can be used in formulations which represent the translational velocities in the local (body-attached) frame

  • input:

    • q0: 6D vector as np.array containing position coordinates and Cardan-Tait/Bryan angles

    • incrementalMotionVector: 6D incremental motion vector as np.array

  • output: 6D vector as np.array containing composed position coordinates and composed Cardan-Tait/Bryan angles

  • author: Stefan Holzinger

Function: CompositionRuleForEulerParameters

CompositionRuleForEulerParameters(q, p)

  • function description: compute composition operation for Euler parameters (unit quaternions) this composition operation is quaternion multiplication, see Terze2016

  • input:

    • q: 4D vector as np.array containing Euler parameters

    • p: 4D vector as np.array containing Euler parameters

  • output: 4D vector as np.array containing composed (multiplied) Euler parameters

  • author: Stefan Holzinger

Function: CompositionRuleForRotationVectors

CompositionRuleForRotationVectors(v0, Omega)

  • function description: compute composition operation for rotation vectors v0 and Omega, see Holzinger2021 The composed vector is NOT mapped into the principal range: its norm lies in [0, 2pi] and may therefore exceed pi. It always describes the correct rotation - ExpSO3 of it is the intended rotation matrix - but it need not be the shortest representative of it. Composing pin with itself, for example, gives a vector of norm 2pi rather than 0; both are the identity rotation. This matches the C++ implementation EXUlie::CompositionRotationVector (src/Linalg/RigidBodyMath.h), which is the verified one used by the Lie group integrator, and is a deliberate decision (#2494): the formulas accept and pass on such noise rather than snapping to a boundary, which keeps every existing result unchanged. A caller who needs the principal range must map it - for a norm w > pi, use (2pi - w) about the negated axis. Note also that accuracy degrades towards w = 2pi, which is the singularity of the tangent operator: TExpSO3Inv() returns entries of order 1e15 there. How much is lost depends on the axis - composing pin with itself is exact to about 1e-15 for n = [1,1,1]/sqrt(3) and to about 4e-8 for n = [0,0,1].

  • input:

    • v0: 3D rotation vector as np.array

    • Omega: 3D (incremental) rotation vector as np.array

  • output: 3D vector as np.array containing composed rotation vector v; norm in [0, 2*pi], see above

  • author: Stefan Holzinger

Relevant Examples (Ex) and TestModels (TM) with weblink to github: LieGroupIntegrationUnitTests.py (TM)

Function: CompositionRuleRotXYZAnglesRotationVector

CompositionRuleRotXYZAnglesRotationVector(alpha0, Omega)

  • function description: compute composition operation for RotXYZ angles, see Holzinger2021

  • input:

    • alpha0: 3D vector as np.array containing RotXYZ angles

    • Omega: 3D vector as np.array containing the (incremental) rotation vector

  • output: 3D vector as np.array containing composed RotXYZ angles

  • author: Stefan Holzinger