Module: rigidBodyUtilities

Advanced utility/mathematical functions for reference frames, rigid body kinematics and dynamics. Useful Euler parameter and Tait-Bryan angle conversion functions are included. A class for rigid body inertia creating and transformation is available.

Author: Johannes Gerstmayr, Stefan Holzinger (rotation vector and Tait-Bryan angles)
Date: 2020-03-10 (created)

Function: ComputeOrthonormalBasisVectors

ComputeOrthonormalBasisVectors(vector0)

function description:

compute orthogonal basis vectors (normal1, normal2) for given vector0 (non-unique solution!); the length of vector0 must not be 1; if vector0 == [0,0,0], then any normal basis is returned
output:

returns [vector0normalized, normal1, normal2], in which vector0normalized is the normalized vector0 (has unit length); all vectors in numpy array format

Function: ComputeOrthonormalBasis

ComputeOrthonormalBasis(vector0)

function description:

compute orthogonal basis, in which the normalized vector0 is the first column and the other columns are normals to vector0 (non-unique solution!); the length of vector0 must not be 1; if vector0 == [0,0,0], then any normal basis is returned
output:

returns A, a rotation matrix, in which the first column is parallel to vector0; A is a 2D numpy array

Function: GramSchmidt

GramSchmidt(vector0, vector1)

function description:

compute Gram-Schmidt projection of given 3D vector 1 on vector 0 and return normalized triad (vector0, vector1, vector0 x vector1)

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

ACFtest.py (TM), sliderCrank3Dbenchmark.py (TM), sliderCrank3Dtest.py (TM)

Function: Skew

Skew(vector)

function description:

compute skew symmetric 3x3-matrix from 3x1- or 1x3-vector

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

leggedRobot.py (Ex), mobileMecanumWheelRobotWithLidar.py (Ex), carRollingDiscTest.py (TM), createRollingDiscPenaltyTest.py (TM), explicitLieGroupIntegratorPythonTest.py (TM), explicitLieGroupIntegratorTest.py (TM), heavyTop.py (TM), laserScannerTest.py (TM)

Function: Skew2Vec

Skew2Vec(skew)

function description:

convert skew symmetric matrix m to vector

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

serialRobotInverseKinematics.py (Ex)

Function: ComputeSkewMatrix

ComputeSkewMatrix(v)

function description:

compute skew matrix from vector or matrix; used for ObjectFFRF and CMS implementation
input:

a vector v in np.array format, containing 3*n components or a matrix with m columns of same shape
output:

if v is a vector, output is (3*n x 3) skew matrix in np.array format; if v is a (n x m) matrix, the output is a (3*n x m) skew matrix in np.array format

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

objectFFRFTest.py (TM)

Function: EulerParameters2G

EulerParameters2G(eulerParameters)

function description:

convert Euler parameters (ep) to G-matrix (=\(\partial \tomega / \partial {\mathbf{p}}_t\))
input:

vector of 4 eulerParameters as list or np.array
output:

3x4 matrix G as np.array

Function: EulerParameters2GLocal

EulerParameters2GLocal(eulerParameters)

function description:

convert Euler parameters (ep) to local G-matrix (=\(\partial \LU{b}{\tomega} / \partial {\mathbf{p}}_t\))
input:

vector of 4 eulerParameters as list or np.array
output:

3x4 matrix G as np.array

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

objectFFRFTest.py (TM), rigidBodyAsUserFunctionTest.py (TM)

Function: EulerParameters2RotationMatrix

EulerParameters2RotationMatrix(eulerParameters)

function description:

compute rotation matrix from eulerParameters
input:

vector of 4 eulerParameters as list or np.array
output:

3x3 rotation matrix as np.array

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

ROSMobileManipulator.py (Ex), stiffFlyballGovernor2.py (Ex), stiffFlyballGovernor.py (TM)

Function: RotationMatrix2EulerParameters

RotationMatrix2EulerParameters(rotationMatrix)

function description:

compute Euler parameters from given rotation matrix
input:

3x3 rotation matrix as list of lists or as np.array
output:

vector of 4 eulerParameters as np.array

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

NGsolvePistonEngine.py (Ex), stiffFlyballGovernor2.py (Ex), perf3DRigidBodies.py (TM), rightAngleFrame.py (TM), stiffFlyballGovernor.py (TM)

Function: AngularVelocity2EulerParameters_t

AngularVelocity2EulerParameters_t(angularVelocity, eulerParameters)

function description:

compute time derivative of Euler parameters from (global) angular velocity vector

note that for Euler parameters \({\mathbf{p}}\), we have \(\tomega={\mathbf{G}} \dot {\mathbf{p}}\) ==> \({\mathbf{G}}^T \tomega = {\mathbf{G}}^T\cdot {\mathbf{G}}\cdot \dot {\mathbf{p}}\) ==> \({\mathbf{G}}^T {\mathbf{G}}=4({\mathbf{I}}_{4 \times 4} - {\mathbf{p}}\cdot {\mathbf{p}}^T)\dot{\mathbf{p}} = 4 ({\mathbf{I}}_{4x4}) \dot {\mathbf{p}}\)
input:

angularVelocity: 3D vector of angular velocity in global frame, as lists or as np.array

eulerParameters: vector of 4 eulerParameters as np.array or list
output:

vector of time derivatives of 4 eulerParameters as np.array

Function: RotationVector2RotationMatrix

RotationVector2RotationMatrix(rotationVector)

function description:

rotaton matrix from rotation vector, see appendix B in
input:

3D rotation vector as list or np.array
output:

3x3 rotation matrix as np.array
notes:

gets inaccurate for very large rotations, \(\phi \\gg 2*\pi\)

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

chatGPTupdate.py (Ex), chatGPTupdate2.py (Ex), stiffFlyballGovernor2.py (Ex), universalJoint.py (Ex), createFunctionsTest.py (TM), explicitLieGroupMBSTest.py (TM), jointArgsTest.py (TM), stiffFlyballGovernor.py (TM)

Function: RotationMatrix2RotationVector

RotationMatrix2RotationVector(rotationMatrix)

function description:

compute rotation vector from rotation matrix
input:

3x3 rotation matrix as list of lists or as np.array
output:

vector of 3 components of rotation vector as np.array

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

explicitLieGroupMBSTest.py (TM)

Function: ComputeRotationAxisFromRotationVector

ComputeRotationAxisFromRotationVector(rotationVector)

function description:

compute rotation axis from given rotation vector
input:

3D rotation vector as np.array
output:

3D vector as np.array representing the rotation axis

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

LieGroupIntegrationUnitTests.py (TM)

Function: RotationVector2G

RotationVector2G(rotationVector)

function description:

convert rotation vector (parameters) (v) to G-matrix (=\(\partial \tomega / \partial \dot {\mathbf{v}}\))
input:

vector of rotation vector (len=3) as list or np.array
output:

3x3 matrix G as np.array

Function: RotationVector2GLocal

RotationVector2GLocal(eulerParameters)

function description:

convert rotation vector (parameters) (v) to local G-matrix (=\(\partial \LU{b}{\tomega} / \partial {\mathbf{v}}_t\))
input:

vector of rotation vector (len=3) as list or np.array
output:

3x3 matrix G as np.array

Function: RotXYZ2RotationMatrix

RotXYZ2RotationMatrix(rot)

function description:

compute rotation matrix from consecutive xyz Rot (Tait-Bryan angles); A=Ax*Ay*Az; rot=[rotX, rotY, rotZ]
input:

3D vector of Tait-Bryan rotation parameters [X,Y,Z] in radiant
output:

3x3 rotation matrix as np.array

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

InverseKinematicsNumericalExample.py (Ex), kinematicTreeAndMBS.py (Ex), stiffFlyballGovernor2.py (Ex), explicitLieGroupMBSTest.py (TM), generalContactImplicit2.py (TM), kinematicTreeTest.py (TM), stiffFlyballGovernor.py (TM)

Function: RotationMatrix2RotXYZ

RotationMatrix2RotXYZ(rotationMatrix)

function description:

convert rotation matrix to xyz Euler angles (Tait-Bryan angles); A=Ax*Ay*Az;
input:

3x3 rotation matrix as list of lists or np.array
output:

vector of Tait-Bryan rotation parameters [X,Y,Z] (in radiant) as np.array
notes:

due to gimbal lock / singularity at rot[1] = pi/2, -pi/2, … the reconstruction of

RotationMatrix2RotXYZ( RotXYZ2RotationMatrix(rot) ) may fail, but

RotXYZ2RotationMatrix( RotationMatrix2RotXYZ( RotXYZ2RotationMatrix(rot) ) ) works always

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

serialRobotInteractiveLimits.py (Ex)

Function: RotXYZ2G

RotXYZ2G(rot)

function description:

compute (global-frame) G-matrix for xyz Euler angles (Tait-Bryan angles) (\(\LU{0}{{\mathbf{G}}} = \partial \LU{0}{\tomega} / \partial \dot \ttheta\))
input:

3D vector of Tait-Bryan rotation parameters [X,Y,Z] in radiant
output:

3x3 matrix G as np.array

Function: RotXYZ2G_t

RotXYZ2G_t(rot, rot_t)

function description:

compute time derivative of (global-frame) G-matrix for xyz Euler angles (Tait-Bryan angles) (\(\LU{0}{{\mathbf{G}}} = \partial \LU{0}{\tomega} / \partial \dot \ttheta\))
input:

rot: 3D vector of Tait-Bryan rotation parameters [X,Y,Z] in radiant

rot_t: 3D vector of time derivative of Tait-Bryan rotation parameters [X,Y,Z] in radiant/s
output:

3x3 matrix G_t as np.array

Function: RotXYZ2GLocal

RotXYZ2GLocal(rot)

function description:

compute local (body-fixed) G-matrix for xyz Euler angles (Tait-Bryan angles) (\(\LU{b}{{\mathbf{G}}} = \partial \LU{b}{\tomega} / \partial \ttheta_t\))
input:

3D vector of Tait-Bryan rotation parameters [X,Y,Z] in radiant
output:

3x3 matrix GLocal as np.array

Function: RotXYZ2GLocal_t

RotXYZ2GLocal_t(rot, rot_t)

function description:

compute time derivative of (body-fixed) G-matrix for xyz Euler angles (Tait-Bryan angles) (\(\LU{b}{{\mathbf{G}}} = \partial \LU{b}{\tomega} / \partial \ttheta_t\))
input:

rot: 3D vector of Tait-Bryan rotation parameters [X,Y,Z] in radiant

rot_t: 3D vector of time derivative of Tait-Bryan rotation parameters [X,Y,Z] in radiant/s
output:

3x3 matrix GLocal_t as np.array

Function: AngularVelocity2RotXYZ_t

AngularVelocity2RotXYZ_t(angularVelocity, rotation)

function description:

compute time derivatives of angles RotXYZ from (global) angular velocity vector and given rotation
input:

angularVelocity: global angular velocity vector as list or np.array

rotation: 3D vector of Tait-Bryan rotation parameters [X,Y,Z] in radiant
output:

time derivative of vector of Tait-Bryan rotation parameters [X,Y,Z] (in radiant) as np.array

Function: RotXYZ2EulerParameters

RotXYZ2EulerParameters(alpha)

function description:

compute four Euler parameters from given RotXYZ angles, see
input:

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

4D vector as np.array containing four Euler parameters

entry zero of output represent the scalar part of Euler parameters

Function: RotationMatrix2RotZYZ

RotationMatrix2RotZYZ(rotationMatrix, flip)

function description:

convert rotation matrix to zyz Euler angles; A=Az*Ay*Az;
input:

rotationMatrix: 3x3 rotation matrix as list of lists or np.array

flip: argument to choose first Euler angle to be in quadrant 2 or 3.
output:

vector of Euler rotation parameters [Z,Y,Z] (in radiant) as np.array
author:

Martin Sereinig
notes:

tested (compared with Robotics, Vision and Control book of P. Corke)

Function: RotationMatrixX

RotationMatrixX(angleRad)

function description:

compute rotation matrix w.r.t. X-axis (first axis)
input:

angle around X-axis in radiant
output:

3x3 rotation matrix as np.array

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

addPrismaticJoint.py (Ex), addRevoluteJoint.py (Ex), graphicsDataExample.py (Ex), mobileMecanumWheelRobotWithLidar.py (Ex), NGsolveCraigBampton.py (Ex), generalContactCylinderTest.py (TM), generalContactCylinderTrigsTest.py (TM), generalContactFrictionTests.py (TM)

Function: RotationMatrixY

RotationMatrixY(angleRad)

function description:

compute rotation matrix w.r.t. Y-axis (second axis)
input:

angle around Y-axis in radiant
output:

3x3 rotation matrix as np.array

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

addPrismaticJoint.py (Ex), addRevoluteJoint.py (Ex), bicycleIftommBenchmark.py (Ex), leggedRobot.py (Ex), mobileMecanumWheelRobotWithLidar.py (Ex), bricardMechanism.py (TM), complexEigenvaluesTest.py (TM), computeODE2AEeigenvaluesTest.py (TM)

Function: RotationMatrixZ

RotationMatrixZ(angleRad)

function description:

compute rotation matrix w.r.t. Z-axis (third axis)
input:

angle around Z-axis in radiant
output:

3x3 rotation matrix as np.array

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

addPrismaticJoint.py (Ex), addRevoluteJoint.py (Ex), bicycleIftommBenchmark.py (Ex), chainDriveExample.py (Ex), fourBarMechanism3D.py (Ex), bricardMechanism.py (TM), carRollingDiscTest.py (TM), complexEigenvaluesTest.py (TM)

Function: RotationMatrix2D

RotationMatrix2D(angleRad)

function description:

compute 2D rotation matrix
input:

angle around out-of-plane axis in radiant
output:

2x2 rotation matrix as np.array

Function: HomogeneousTransformation

HomogeneousTransformation(A, r)

function description:

compute HT matrix from rotation matrix A and translation vector r

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

humanRobotInteraction.py (Ex), InverseKinematicsNumericalExample.py (Ex), kinematicTreeAndMBS.py (Ex), NGsolveCraigBampton.py (Ex), NGsolvePistonEngine.py (Ex)

Function: HTtranslate

HTtranslate(r)

function description:

HT for translation with vector r

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

humanRobotInteraction.py (Ex), InverseKinematicsNumericalExample.py (Ex), kinematicTreeAndMBS.py (Ex), kinematicTreePendulum.py (Ex), openAIgymNLinkAdvanced.py (Ex), createKinematicTreeTest.py (TM), kinematicTreeAndMBStest.py (TM), kinematicTreeConstraintTest.py (TM)

Function: HTtranslateX

HTtranslateX(x)

function description:

HT for translation along x axis with value x

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

kinematicTreeAndMBStest.py (TM)

Function: HTtranslateY

HTtranslateY(y)

function description:

HT for translation along y axis with value y

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

kinematicTreePendulum.py (Ex), openAIgymNLinkAdvanced.py (Ex), openAIgymNLinkContinuous.py (Ex), kinematicTreeAndMBStest.py (TM), kinematicTreeConstraintTest.py (TM)

Function: HTtranslateZ

HTtranslateZ(z)

function description:

HT for translation along z axis with value z

Function: HT0

HT0()

function description:

identity HT:

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

humanRobotInteraction.py (Ex), kinematicTreeAndMBS.py (Ex), kinematicTreePendulum.py (Ex), openAIgymNLinkAdvanced.py (Ex), openAIgymNLinkContinuous.py (Ex), kinematicTreeAndMBStest.py (TM), kinematicTreeConstraintTest.py (TM)

Function: HTrotateX

HTrotateX(angle)

function description:

HT for rotation around axis X (first axis)

Function: HTrotateY

HTrotateY(angle)

function description:

HT for rotation around axis X (first axis)

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

kinematicTreeAndMBStest.py (TM)

Function: HTrotateZ

HTrotateZ(angle)

function description:

HT for rotation around axis X (first axis)

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

ROSMobileManipulator.py (Ex), kinematicTreeAndMBStest.py (TM)

Function: HT2translation

HT2translation(T)

function description:

return translation part of HT

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

kinematicTreeAndMBS.py (Ex), serialRobotFlexible.py (Ex), serialRobotInteractiveLimits.py (Ex), serialRobotInverseKinematics.py (Ex), serialRobotKinematicTree.py (Ex), kinematicTreeAndMBStest.py (TM), movingGroundRobotTest.py (TM), serialRobotTest.py (TM)

Function: HT2rotationMatrix

HT2rotationMatrix(T)

function description:

return rotation matrix of HT

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

kinematicTreeAndMBS.py (Ex), kinematicTreeAndMBStest.py (TM)

Function: InverseHT

InverseHT(T)

function description:

return inverse HT such that inv(T)*T = np.eye(4)

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

serialRobotKinematicTree.py (Ex)

Function: RotationX2T66

RotationX2T66(angle)

function description:

compute 6x6 coordinate transformation matrix for rotation around X axis; output: first 3 components for rotation, second 3 components for translation! See Featherstone / Handbook of robotics

Function: RotationY2T66

RotationY2T66(angle)

function description:

compute 6x6 transformation matrix for rotation around Y axis; output: first 3 components for rotation, second 3 components for translation

Function: RotationZ2T66

RotationZ2T66(angle)

function description:

compute 6x6 transformation matrix for rotation around Z axis; output: first 3 components for rotation, second 3 components for translation

Function: Translation2T66

Translation2T66(translation3D)

function description:

compute 6x6 transformation matrix for translation according to 3D vector translation3D; output: first 3 components for rotation, second 3 components for translation!

Function: TranslationX2T66

TranslationX2T66(translation)

function description:

compute 6x6 transformation matrix for translation along X axis; output: first 3 components for rotation, second 3 components for translation!

Function: TranslationY2T66

TranslationY2T66(translation)

function description:

compute 6x6 transformation matrix for translation along Y axis; output: first 3 components for rotation, second 3 components for translation!

Function: TranslationZ2T66

TranslationZ2T66(translation)

function description:

compute 6x6 transformation matrix for translation along Z axis; output: first 3 components for rotation, second 3 components for translation!

Function: T66toRotationTranslation

T66toRotationTranslation(T66)

function description:

convert 6x6 coordinate transformation (Plücker transform) into rotation and translation
input:

T66 given as 6x6 numpy array
output:

[A, v] with 3x3 rotation matrix A and 3D translation vector v

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

kinematicTreeAndMBS.py (Ex)

Function: InverseT66toRotationTranslation

InverseT66toRotationTranslation(T66)

function description:

convert inverse 6x6 coordinate transformation (Plücker transform) into rotation and translation
input:

inverse T66 given as 6x6 numpy array
output:

[A, v] with 3x3 rotation matrix A and 3D translation vector v

Function: RotationTranslation2T66

RotationTranslation2T66(A, v)

function description:

convert rotation and translation into 6x6 coordinate transformation (Plücker transform)
input:

A: 3x3 rotation matrix A

v: 3D translation vector v
output:

return 6x6 transformation matrix ‘T66’

Function: RotationTranslation2T66Inverse

RotationTranslation2T66Inverse(A, v)

function description:

convert rotation and translation into INVERSE 6x6 coordinate transformation (Plücker transform)
input:

A: 3x3 rotation matrix A

v: 3D translation vector v
output:

return 6x6 transformation matrix ‘T66’

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

kinematicTreeAndMBS.py (Ex)

Function: T66toHT

T66toHT(T66)

function description:

convert 6x6 coordinate transformation (Plücker transform) into 4x4 homogeneous transformation; NOTE that the homogeneous transformation is the inverse of what is computed in function pluho() of Featherstone
input:

T66 given as 6x6 numpy array
output:

homogeneous transformation (4x4 numpy array)

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

kinematicTreeAndMBS.py (Ex)

Function: HT2T66Inverse

HT2T66Inverse(T)

function description:

convert 4x4 homogeneous transformation into 6x6 coordinate transformation (Plücker transform); NOTE that the homogeneous transformation is the inverse of what is computed in function pluho() of Featherstone
output:

input: T66 (6x6 numpy array)

Function: InertiaTensor2Inertia6D

InertiaTensor2Inertia6D(inertiaTensor)

function description:

convert a 3x3 matrix (list or numpy array) into a list with 6 inertia components, sorted as J00, J11, J22, J12, J02, J01

Function: Inertia6D2InertiaTensor

Inertia6D2InertiaTensor(inertia6D)

function description:

convert a list or numpy array with 6 inertia components (sorted as [J00, J11, J22, J12, J02, J01]) (list or numpy array) into a 3x3 matrix (np.array)

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

rigidBodyAsUserFunctionTest.py (TM)

Function: StrNodeType2NodeType

StrNodeType2NodeType(sNodeType)

function description:

convert string into exudyn.NodeType; call e.g. with ‘NodeType.RotationEulerParameters’ or ‘RotationEulerParameters’
notes:

function is not very fast, so should be avoided in time-critical situations

Function: GetRigidBodyNode

GetRigidBodyNode(nodeType, position = [0,0,0], velocity = [0,0,0], rotationMatrix = [], rotationParameters = [], angularVelocity = [0,0,0])

function description:

get node item interface according to nodeType, using initialization with position, velocity, angularVelocity and rotationMatrix
input:

nodeType: a node type according to exudyn.NodeType, or a string of it, e.g., ‘NodeType.RotationEulerParameters’ (fastest, but additional algebraic constraint equation), ‘NodeType.RotationRxyz’ (Tait-Bryan angles, singularity for second angle at +/- 90 degrees), ‘NodeType.RotationRotationVector’ (used for Lie group integration)

position: reference position as list or numpy array with 3 components (in global/world frame)

velocity: initial translational velocity as list or numpy array with 3 components (in global/world frame)

rotationMatrix: 3x3 list or numpy matrix to define reference rotation; use EITHER rotationMatrix=[[…],[…],[…]] (while rotationParameters=[]) or rotationParameters=[…] (while rotationMatrix=[])

rotationParameters: reference rotation parameters; use EITHER rotationMatrix=[[…],[…],[…]] (while rotationParameters=[]) or rotationParameters=[…] (while rotationMatrix=[])

angularVelocity: initial angular velocity as list or numpy array with 3 components (in global/world frame)
output:

returns list containing node number and body number: [nodeNumber, bodyNumber]

Function: AddRigidBody

AddRigidBody(mainSys, inertia, nodeType = exu.NodeType.RotationEulerParameters, position = [0,0,0], velocity = [0,0,0], rotationMatrix = [], rotationParameters = [], angularVelocity = [0,0,0], gravity = [0,0,0], graphicsDataList = [])

function description:

DEPRECATED: adds a node (with str(exu.NodeType. …)) and body for a given rigid body; all quantities (esp. velocity and angular velocity) are given in global coordinates!
input:

inertia: an inertia object as created by class RigidBodyInertia; containing mass, COM and inertia

nodeType: a node type according to exudyn.NodeType, or a string of it, e.g., ‘NodeType.RotationEulerParameters’ (fastest, but additional algebraic constraint equation), ‘NodeType.RotationRxyz’ (Tait-Bryan angles, singularity for second angle at +/- 90 degrees), ‘NodeType.RotationRotationVector’ (used for Lie group integration)

position: reference position as list or numpy array with 3 components (in global/world frame)

velocity: initial translational velocity as list or numpy array with 3 components (in global/world frame)

rotationMatrix: 3x3 list or numpy matrix to define reference rotation; use EITHER rotationMatrix=[[…],[…],[…]] (while rotationParameters=[]) or rotationParameters=[…] (while rotationMatrix=[])

rotationParameters: reference rotation parameters; use EITHER rotationMatrix=[[…],[…],[…]] (while rotationParameters=[]) or rotationParameters=[…] (while rotationMatrix=[])

angularVelocity: initial angular velocity as list or numpy array with 3 components (in global/world frame)

gravity: if provided as list or numpy array with 3 components, it adds gravity force to the body at the COM, i.e., fAdd = m*gravity

graphicsDataList: list of graphicsData objects to define appearance of body
output:

returns list containing node number and body number: [nodeNumber, bodyNumber]
notes:

DEPRECATED and will be removed; use MainSystem.CreateRigidBody(…) instead!

Function: AddRevoluteJoint

AddRevoluteJoint(mbs, body0, body1, point, axis, useGlobalFrame = True, showJoint = True, axisRadius = 0.1, axisLength = 0.4)

function description:

DEPRECATED (use MainSystem function instead): add revolute joint between two bodies; definition of joint position and axis in global coordinates (alternatively in body0 local coordinates) for reference configuration of bodies; all markers, markerRotation and other quantities are automatically computed
input:

mbs: the MainSystem to which the joint and markers shall be added

body0: a object number for body0, must be rigid body or ground object

body1: a object number for body1, must be rigid body or ground object

point: a 3D vector as list or np.array containing the global center point of the joint in reference configuration

axis: a 3D vector as list or np.array containing the global rotation axis of the joint in reference configuration

useGlobalFrame: if False, the point and axis vectors are defined in the local coordinate system of body0
output:

returns list [oJoint, mBody0, mBody1], containing the joint object number, and the two rigid body markers on body0/1 for the joint
notes:

DEPRECATED and will be removed; use MainSystem.CreateRevoluteJoint(…) instead!

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

openVRengine.py (Ex)

Function: AddPrismaticJoint

AddPrismaticJoint(mbs, body0, body1, point, axis, useGlobalFrame = True, showJoint = True, axisRadius = 0.1, axisLength = 0.4)

function description:

DEPRECATED (use MainSystem function instead): add prismatic joint between two bodies; definition of joint position and axis in global coordinates (alternatively in body0 local coordinates) for reference configuration of bodies; all markers, markerRotation and other quantities are automatically computed
input:

mbs: the MainSystem to which the joint and markers shall be added

body0: a object number for body0, must be rigid body or ground object

body1: a object number for body1, must be rigid body or ground object

point: a 3D vector as list or np.array containing the global center point of the joint in reference configuration

axis: a 3D vector as list or np.array containing the global translation axis of the joint in reference configuration

useGlobalFrame: if False, the point and axis vectors are defined in the local coordinate system of body0
output:

returns list [oJoint, mBody0, mBody1], containing the joint object number, and the two rigid body markers on body0/1 for the joint
notes:

DEPRECATED and will be removed; use MainSystem.CreatePrismaticJoint(…) instead!

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

openVRengine.py (Ex)

CLASS TreeLink (in module rigidBodyUtilities)

class description:

helper class for CreateKinematicTree, representing a link on a joint within a kinematic tree

example:

link3 = TreeLink(linkInertia = InertiaCuboid(2800, [0.25,0.08,0.08]).Translated([0.125,0,0]),
                 jointType = =exu.JointType.RevoluteZ,
                 parent = 1,
                 graphicsData = graphics.Brick(centerPoint=[0.125,0,0], size=[0.25,0.08,0.08],
                                               color=graphics.color.blue),
                 )

Class function: init

__init__(self, linkInertia, jointType = exu.JointType.RevoluteZ, jointHT = HT0(), parent = None, PDcontrol = None, graphicsDataList = None)

classFunction:

initialize inertia
input:

linkInertia: RigidBodyInertia class, containing mass, inertia, and COM

jointHT: transformation from previous link to this link’s joint

parent: index to parent link; if parent link is ground, use -1; if all parents in a serial kinematic tree are None, parent indices are computed automatically

PDcontrol: tuple of PD control parameters

graphicsData: graphicsDataList link; None automatically adds a suitable graphical object from next joint to this joint; use empty list [] to add no graphics for link

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

createKinematicTreeTest.py (TM)

CLASS RigidBodyInertia (in module rigidBodyUtilities)

class description:

helper class for rigid body inertia (see also derived classes Inertia…). Provides a structure to define mass, inertia and center of mass (COM) of a rigid body. The inertia tensor and center of mass must correspond when initializing the body!

notes:

in the default mode, inertiaTensorAtCOM = False, the inertia tensor must be provided with respect to the reference point; otherwise, it is given at COM; internally, the inertia tensor is always with respect to the reference point, not w.r.t. to COM!
example:

i0 = RigidBodyInertia(10,np.diag([1,2,3]))
i1 = i0.Rotated(RotationMatrixX(np.pi/2))
i2 = i1.Translated([1,0,0])

Class function: init

__init__(self, mass = 0, inertiaTensor = np.zeros([3,3]), com = np.zeros(3), inertiaTensorAtCOM = False)

classFunction:

initialize RigidBodyInertia with scalar mass, 3x3 inertiaTensor (w.r.t. reference point!!!) and center of mass com
input:

mass: mass of rigid body (dimensions need to be consistent, should be in SI-units)

inertiaTensor: tensor given w.r.t.reference point, NOT w.r.t.center of mass!

com: center of mass relative to reference point, in same coordinate system as inertiaTensor

inertiaTensorAtCOM: bool flag: if False (default), the inertiaTensor has to be provided w.r.t. the reference point; if True, it has to be provided at the center of mass

Class function: add

__add__(self, otherBodyInertia)

classFunction:

add (+) operator allows adding another inertia information with SAME local coordinate system and reference point!

only inertias with same center of rotation can be added!
example:

J = InertiaSphere(2,0.1) + InertiaRodX(1,2)

Class function: iadd

__iadd__(self, otherBodyInertia)

classFunction:

+= operator allows adding another inertia information with SAME local coordinate system and reference point!

only inertias with same center of rotation can be added!
example:

J = InertiaSphere(2,0.1)
J += InertiaRodX(1,2)

Class function: SetWithCOMinertia

SetWithCOMinertia(self, mass, inertiaTensorCOM, com)

classFunction:

set RigidBodyInertia with scalar mass, 3x3 inertiaTensor (w.r.t.com) and center of mass com
input:

mass: mass of rigid body (dimensions need to be consistent, should be in SI-units)

inertiaTensorCOM: tensor given w.r.t.reference point, NOT w.r.t.center of mass!

com: center of mass relative to reference point, in same coordinate system as inertiaTensor

Class function: Inertia

Inertia(self)

classFunction:

returns 3x3 inertia tensor with respect to chosen reference point (not necessarily COM)

Class function: InertiaCOM

InertiaCOM(self)

classFunction:

returns 3x3 inertia tensor with respect to COM

Class function: COM

COM(self)

classFunction:

returns center of mass (COM) w.r.t. chosen reference point

Class function: Mass

Mass(self)

classFunction:

returns mass

Class function: Translated

Translated(self, vec)

classFunction:

returns a RigidBodyInertia with center of mass com shifted by vec; \(\ra\) transforms the returned inertiaTensor to the new center of rotation

Class function: Rotated

Rotated(self, rot)

classFunction:

returns a RigidBodyInertia rotated by 3x3 rotation matrix rot, such that for a given J, the new inertia tensor reads Jnew = rot*J*rot.T
notes:

only allowed if COM=0 !

Class function: Transformed

Transformed(self, HT)

classFunction:

return rigid body inertia transformed by homogeneous transformation HT

Class function: GetInertia6D

GetInertia6D(self)

classFunction:

get vector with 6 inertia components (Jxx, Jyy, Jzz, Jyz, Jxz, Jxy) w.r.t. to reference point (not necessarily the COM), as needed in ObjectRigidBody

Class function: GetTypeName

GetTypeName(self)

classFunction:

which returns str of type (‘InertiaCylinder’, ‘InertiaCuboid’, …)

Class function: GetSpecialData

GetSpecialData(self)

classFunction:

returns dictionary with further data of inertia, like cylinder radius, etc.

Class function: GetGraphics

GetGraphics(self, color, nTiles = None, roundness = None)

classFunction:

get graphicsData object from inertia; this simplifies the rigid body creation process and allows to check for consistency; currently does not include HT-rotations!

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

bicycleIftommBenchmark.py (Ex), humanRobotInteraction.py (Ex), openAIgymNLinkAdvanced.py (Ex), openAIgymNLinkContinuous.py (Ex), reinforcementLearningRobot.py (Ex), createRollingDiscPenaltyTest.py (TM), rigidBody2Dtest.py (TM), rigidBodyCOMtest.py (TM)

CLASS InertiaCuboid(RigidBodyInertia) (in module rigidBodyUtilities)

class description:

create RigidBodyInertia with moment of inertia and mass of a cuboid with density and side lengths sideLengths along local axes 1, 2, 3; inertia w.r.t. center of mass, com=[0,0,0]

example:

InertiaCuboid(density=1000,sideLengths=[1,0.1,0.1])

Class function: init

__init__(self, density, sideLengths)

classFunction:

initialize inertia

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

addPrismaticJoint.py (Ex), addRevoluteJoint.py (Ex), ANCFrotatingCable2D.py (Ex), bungeeJump.py (Ex), camFollowerExample.py (Ex), bricardMechanism.py (TM), carRollingDiscTest.py (TM), complexEigenvaluesTest.py (TM)

CLASS InertiaRodX(RigidBodyInertia) (in module rigidBodyUtilities)

class description:

create RigidBodyInertia with moment of inertia and mass of a rod with mass m and length L in local 1-direction (x-direction); inertia w.r.t. center of mass, com=[0,0,0]

Class function: init

__init__(self, mass, length)

classFunction:

initialize inertia with mass and length of rod

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

fourBarMechanismIftomm.py (TM)

CLASS InertiaMassPoint(RigidBodyInertia) (in module rigidBodyUtilities)

class description:

create RigidBodyInertia with moment of inertia and mass of mass point with given ‘mass’; inertia w.r.t. center of mass, com=[0,0,0]; note that the inertia tensor gives zero and cannot be directly used in rigid bodies, however, it can be used to be added to another inertia tensor (e.g. to add unbalance)

Class function: init

__init__(self, mass)

classFunction:

initialize inertia with mass of point

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

stiffFlyballGovernor2.py (Ex), stiffFlyballGovernorKT.py (Ex), stiffFlyballGovernor.py (TM)

CLASS InertiaSphere(RigidBodyInertia) (in module rigidBodyUtilities)

class description:

create RigidBodyInertia with moment of inertia and mass of sphere with mass and radius; inertia w.r.t. center of mass, com=[0,0,0]

Class function: init

__init__(self, mass = None, radius = None, density = None)

classFunction:

initialize inertia with mass and radius of sphere

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

contactCurveWithLongCurve.py (Ex), graphicsDataExample.py (Ex), newtonsCradle.py (Ex), particleClusters.py (Ex), particlesSilo.py (Ex), contactSphereSphereTest.py (TM), contactSphereSphereTestEAPM.py (TM), createFunctionsTest.py (TM)

CLASS InertiaHollowSphere(RigidBodyInertia) (in module rigidBodyUtilities)

class description:

create RigidBodyInertia with moment of inertia and mass of hollow sphere with mass (concentrated at circumference) and radius; inertia w.r.t. center of mass, com=0

Class function: init

__init__(self, mass, radius)

classFunction:

initialize inertia with mass and (inner==outer) radius of hollow sphere

CLASS InertiaCylinder(RigidBodyInertia) (in module rigidBodyUtilities)

class description:

create RigidBodyInertia with moment of inertia and mass of cylinder with density, length and outerRadius; axis defines the orientation of the cylinder axis (0=x-axis, 1=y-axis, 2=z-axis); for hollow cylinder use innerRadius != 0; inertia w.r.t. center of mass, com=[0,0,0]

Class function: init

__init__(self, density, length, outerRadius, axis, innerRadius = 0)

classFunction:

initialize inertia with density, length, outer radius, axis (0=x-axis, 1=y-axis, 2=z-axis) and optional inner radius (for hollow cylinder)

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

ballBearningModel.py (Ex), camFollowerExample.py (Ex), chainDriveExample.py (Ex), gyroStability.py (Ex), involuteGearGraphics.py (Ex), ANCFbeltDrive.py (TM), ANCFgeneralContactCircle.py (TM), ballBearingTest.py (TM)

Module: rigidBodyUtilities

Function: ComputeOrthonormalBasisVectors

Function: ComputeOrthonormalBasis

Function: GramSchmidt

Function: Skew

Function: Skew2Vec

Function: ComputeSkewMatrix

Function: EulerParameters2G

Function: EulerParameters2GLocal

Function: EulerParameters2RotationMatrix

Function: RotationMatrix2EulerParameters

Function: AngularVelocity2EulerParameters_t

Function: RotationVector2RotationMatrix

Function: RotationMatrix2RotationVector

Function: ComputeRotationAxisFromRotationVector

Function: RotationVector2G

Function: RotationVector2GLocal

Function: RotXYZ2RotationMatrix

Function: RotationMatrix2RotXYZ

Function: RotXYZ2G

Function: RotXYZ2G_t

Function: RotXYZ2GLocal

Function: RotXYZ2GLocal_t

Function: AngularVelocity2RotXYZ_t

Function: RotXYZ2EulerParameters

Function: RotationMatrix2RotZYZ

Function: RotationMatrixX

Function: RotationMatrixY

Function: RotationMatrixZ

Function: RotationMatrix2D

Function: HomogeneousTransformation

Function: HTtranslate

Function: HTtranslateX

Function: HTtranslateY

Function: HTtranslateZ

Function: HT0

Function: HTrotateX

Function: HTrotateY

Function: HTrotateZ

Function: HT2translation

Function: HT2rotationMatrix

Function: InverseHT

Function: RotationX2T66

Function: RotationY2T66

Function: RotationZ2T66

Function: Translation2T66

Function: TranslationX2T66

Function: TranslationY2T66

Function: TranslationZ2T66

Function: T66toRotationTranslation

Function: InverseT66toRotationTranslation

Function: RotationTranslation2T66

Function: RotationTranslation2T66Inverse

Function: T66toHT

Function: HT2T66Inverse

Function: InertiaTensor2Inertia6D

Function: Inertia6D2InertiaTensor

Function: StrNodeType2NodeType

Function: GetRigidBodyNode

Function: AddRigidBody

Function: AddRevoluteJoint

Function: AddPrismaticJoint

CLASS TreeLink (in module rigidBodyUtilities)

Class function: __init__

CLASS RigidBodyInertia (in module rigidBodyUtilities)

Class function: __init__

Class function: __add__

Class function: __iadd__

Class function: SetWithCOMinertia

Class function: Inertia

Class function: InertiaCOM

Class function: COM

Class function: Mass

Class function: Translated

Class function: Rotated

Class function: Transformed

Class function: GetInertia6D

Class function: GetTypeName

Class function: GetSpecialData

Class function: GetGraphics

Class function: init

Class function: init

Class function: add

Class function: iadd

Class function: init

Class function: init

Class function: init

Class function: init

Class function: init

Class function: init