homogeneousTransformationInterfaceTest.py

You can view and download this file on Github: homogeneousTransformationInterfaceTest.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  exudyn.HT as the place for rigid body transformations (#2810): the rotation from and to Euler
  5#           parameters, Tait-Bryan angles Rxyz and the rotation vector, against exudyn.rigidBodyUtilities;
  6#           the coordinates of the rigid body nodes, the same as CreateRigidBody(referenceHT=...) gives its
  7#           node; Relative, InterpolateSO3 and InterpolateSE3, RotationAngle and RotationAxis,
  8#           SetRotationAxis, the Set functions returning the HT, and HasNoRotation for a unit matrix;
  9#           LogSE3/SetExpSE3 against exudyn.lieGroupBasics and LogR3xSO3/SetExpR3xSO3 (#2819); numpy
 10#           indexing H[0:3,3] for reading and writing, H1@H2 and copies, as for a 4x4 numpy array (#2821).
 11#
 12# Author:   Johannes Gerstmayr
 13# Date:     2026-10-03
 14#
 15# Copyright:This file is part of Exudyn. Exudyn is free software. You can redistribute it and/or modify it under the terms of the Exudyn license. See 'LICENSE.txt' for more details.
 16#
 17#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 18
 19import exudyn as exu
 20from exudyn.utilities import *
 21import numpy as np
 22
 23testIsActive = exu.sys.get('testIsActive', False)
 24
 25#ONLY the regular module raises for invalid input: exudynCPPfast compiles these checks away by design, so the
 26#must-raise cases below are not judged there (#2911)
 27hasRangeChecks = '[FAST]' not in exu.config.Version(True)
 28
 29errors = 0
 30def Check(value, reference, tolerance=1e-12, what=''):
 31    global errors
 32    if np.linalg.norm(np.array(value, dtype=float) - np.array(reference, dtype=float)) > tolerance:
 33        errors += 1
 34        exu.Print('homogeneousTransformationInterfaceTest: failed', what, value, reference)
 35
 36A = RotationMatrixX(0.3) @ RotationMatrixY(-0.7) @ RotationMatrixZ(1.1)
 37p = np.array([0.4, -1.2, 2.5])
 38H = exu.HT(rotation=A, translation=p)
 39
 40#the rotation parameters, against rigidBodyUtilities
 41Check(H.GetEP(), RotationMatrix2EulerParameters(A), what='GetEP')
 42Check(H.GetRxyz(), RotationMatrix2RotXYZ(A), what='GetRxyz')
 43Check(H.GetRotationVector(), RotationMatrix2RotationVector(A), what='GetRotationVector')
 44Check(exu.HT(eulerParameters=RotationMatrix2EulerParameters(A)).rotation, A, what='HT(eulerParameters)')
 45Check(exu.HT(Rxyz=[0.3, -0.7, 1.1]).rotation, RotXYZ2RotationMatrix([0.3, -0.7, 1.1]), what='HT(Rxyz)')
 46Check(exu.HT(rotationVector=[0.2, 0.5, -0.1]).rotation, RotationVector2RotationMatrix([0.2, 0.5, -0.1]), what='HT(rotationVector)')
 47
 48#the coordinates of the nodes, both ways
 49Check(H.GetCoordinatesEP(), list(p) + list(RotationMatrix2EulerParameters(A)), what='GetCoordinatesEP')
 50for (get, put) in [('GetCoordinatesEP', 'SetCoordinatesEP'), ('GetCoordinatesRxyz', 'SetCoordinatesRxyz'),
 51                   ('GetCoordinatesRotationVector', 'SetCoordinatesRotationVector')]:
 52    H2 = getattr(exu.HT(), put)(getattr(H, get)())
 53    Check(H2.HT44(), H.HT44(), what=put)
 54
 55#the same reference coordinates as CreateRigidBody gives its node
 56SC = exu.SystemContainer()
 57mbs = SC.AddSystem()
 58for (nodeType, get) in [(exu.NodeType.RotationEulerParameters, 'GetCoordinatesEP'),
 59                        (exu.NodeType.RotationRxyz, 'GetCoordinatesRxyz'),
 60                        (exu.NodeType.RotationRotationVector, 'GetCoordinatesRotationVector')]:
 61    body = mbs.CreateRigidBody(inertia=InertiaCuboid(1000, [0.1, 0.2, 0.3]), referenceHT=H, nodeType=nodeType,
 62                               returnDict=True)
 63    Check(mbs.GetNodeParameter(body['nodeNumber'], 'referenceCoordinates'), getattr(H, get)(), 1e-12, 'CreateRigidBody ' + get)
 64
 65#Relative and the interpolations
 66H1 = exu.HT(Rxyz=[-0.2, 0.4, 0.9], translation=[1, 2, -0.5])
 67Check(H.Relative(H1).HT44(), (H.Inverse() * H1).HT44(), what='Relative')
 68for interpolate in ['InterpolateSO3', 'InterpolateSE3']:
 69    Check(getattr(H, interpolate)(H1, 0.).HT44(), H.HT44(), what=interpolate + ' 0')
 70    Check(getattr(H, interpolate)(H1, 1.).HT44(), H1.HT44(), 1e-12, interpolate + ' 1')
 71Hhalf = H.InterpolateSO3(H1, 0.5)
 72Check(Hhalf.translation, 0.5 * (p + H1.translation), what='InterpolateSO3 translation')
 73Check(H.Relative(Hhalf).RotationAngle(), 0.5 * H.Relative(H1).RotationAngle(), what='InterpolateSO3 angle')
 74HhalfSE3 = H.InterpolateSE3(H1, 0.5)
 75Check((H * H.Relative(HhalfSE3) * H.Relative(HhalfSE3)).HT44(), H1.HT44(), 1e-12, 'InterpolateSE3 halfway') #constant twist: two half steps
 76
 77#the logarithm and the exponential map on SE(3) and R3xSO(3), against lieGroupBasics (#2819)
 78from exudyn.lieGroupBasics import ExpSE3, LogSE3, Skew2Vec
 79v = H.LogSE3()
 80logH = LogSE3(H.HT44())                                       #4x4: the skew matrix of Omega and U
 81Check(v, list(logH[0:3, 3]) + list(Skew2Vec(logH[0:3, 0:3])), what='LogSE3')
 82Check(exu.HT().SetExpSE3(v).HT44(), H.HT44(), what='SetExpSE3(LogSE3())')
 83Check(exu.HT().SetExpSE3([0.1, 0.2, -0.3, 0.4, -0.1, 0.2]).HT44(), ExpSE3(np.array([0.1, 0.2, -0.3, 0.4, -0.1, 0.2])), what='SetExpSE3')
 84Check((H * exu.HT().SetExpSE3(0.5 * H.Relative(H1).LogSE3())).HT44(), HhalfSE3.HT44(), what='SetExpSE3 as InterpolateSE3')
 85Check(H.LogR3xSO3(), H.GetCoordinatesRotationVector(), what='LogR3xSO3')
 86Check(exu.HT().SetExpR3xSO3(H.LogR3xSO3()).HT44(), H.HT44(), what='SetExpR3xSO3(LogR3xSO3())')
 87
 88#angle and axis
 89H3 = exu.HT().SetRotationAxis([0, 3, 4], 0.8) #the Set functions return the HT
 90Check(H3.RotationAngle(), 0.8, what='RotationAngle')
 91Check(H3.RotationAxis(), [0, 0.6, 0.8], what='RotationAxis')
 92Check(exu.HT().RotationAxis(raiseError=False), [0, 0, 0], what='RotationAxis of no rotation')
 93if hasRangeChecks:
 94    try:
 95        exu.HT().RotationAxis()
 96        errors += 1 #must raise
 97    except Exception:
 98        pass
 99
100#a copy of an HT (#2824)
101H5 = exu.HT(H)
102H5.translation = [0, 0, 0]
103Check(H.translation, p, what='HT(H) is a copy')
104Check(H5.rotation, A, what='HT(H) rotation')
105
106#numpy indexing, @ and copies, as for a 4x4 numpy array (#2821)
107import copy
108Check(H[0:3,3], p, what='H[0:3,3]')
109Check(H[2][3], p[2], what='H[2][3]')
110H6 = exu.HT(H)
111H6[0:3,3] = [1, 2, 3]
112Check(H6.translation, [1, 2, 3], what='H[0:3,3] = value')
113if hasRangeChecks:
114    try:
115        H6[3,3] = 2
116        errors += 1 #the last row must raise
117    except Exception:
118        pass
119Check((H @ H1).HT44(), H.HT44() @ H1.HT44(), what='H@H1')
120Check(H @ [0.1, 0.2, 0.3, 1], H.HT44() @ [0.1, 0.2, 0.3, 1], what='H@x')
121Check(copy.deepcopy([H])[0].HT44(), H.HT44(), what='deepcopy')
122
123#chained Set functions, keywords that keep the other part, HasNoRotation
124H4 = exu.HT().SetRotationZ(0.5).Set(translation=[1, 2, 3])
125Check(H4.HT44(), HTtranslate([1, 2, 3]) @ HTrotateZ(0.5), what='chained Set')
126if exu.HT(rotation=np.eye(3), translation=[1, 0, 0]).HasNoRotation() is not True or H4.HasNoRotation():
127    errors += 1
128if hasRangeChecks:
129    try:
130        exu.HT(Rxyz=[0, 0, 1], rotationVector=[0, 0, 1])
131        errors += 1 #two rotations must raise
132    except Exception:
133        pass
134
135exu.Print('homogeneousTransformationInterfaceTest: errors', errors)
136u = errors + np.sum(H.GetCoordinatesEP()) + np.sum(HhalfSE3.HT44()) + H3.RotationAngle()
137exu.Print('solution of homogeneousTransformationInterfaceTest=', u)
138
139exu.sys['testResult'] = u