<!-- GENERATED by tools/generators/examplesDocsEmitter.py from python/ - do not edit -->
(testmodels-homogeneoustransformationinterfacetest)=
# homogeneousTransformationInterfaceTest.py

You can view and download this file on Github: [homogeneousTransformationInterfaceTest.py](https://github.com/jgerstmayr/EXUDYN/tree/master/python/TestModels/homogeneousTransformationInterfaceTest.py)

```{code-block} python
:linenos:

#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details:  exudyn.HT as the place for rigid body transformations (#2810): the rotation from and to Euler
#           parameters, Tait-Bryan angles Rxyz and the rotation vector, against exudyn.rigidBodyUtilities;
#           the coordinates of the rigid body nodes, the same as CreateRigidBody(referenceHT=...) gives its
#           node; Relative, InterpolateSO3 and InterpolateSE3, RotationAngle and RotationAxis,
#           SetRotationAxis, the Set functions returning the HT, and HasNoRotation for a unit matrix;
#           LogSE3/SetExpSE3 against exudyn.lieGroupBasics and LogR3xSO3/SetExpR3xSO3 (#2819); numpy
#           indexing H[0:3,3] for reading and writing, H1@H2 and copies, as for a 4x4 numpy array (#2821).
#
# Author:   Johannes Gerstmayr
# Date:     2026-10-03
#
# 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.
#
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

import exudyn as exu
from exudyn.utilities import *
import numpy as np

testIsActive = exu.sys.get('testIsActive', False)

#ONLY the regular module raises for invalid input: exudynCPPfast compiles these checks away by design, so the
#must-raise cases below are not judged there (#2911)
hasRangeChecks = '[FAST]' not in exu.config.Version(True)

errors = 0
def Check(value, reference, tolerance=1e-12, what=''):
    global errors
    if np.linalg.norm(np.array(value, dtype=float) - np.array(reference, dtype=float)) > tolerance:
        errors += 1
        exu.Print('homogeneousTransformationInterfaceTest: failed', what, value, reference)

A = RotationMatrixX(0.3) @ RotationMatrixY(-0.7) @ RotationMatrixZ(1.1)
p = np.array([0.4, -1.2, 2.5])
H = exu.HT(rotation=A, translation=p)

#the rotation parameters, against rigidBodyUtilities
Check(H.GetEP(), RotationMatrix2EulerParameters(A), what='GetEP')
Check(H.GetRxyz(), RotationMatrix2RotXYZ(A), what='GetRxyz')
Check(H.GetRotationVector(), RotationMatrix2RotationVector(A), what='GetRotationVector')
Check(exu.HT(eulerParameters=RotationMatrix2EulerParameters(A)).rotation, A, what='HT(eulerParameters)')
Check(exu.HT(Rxyz=[0.3, -0.7, 1.1]).rotation, RotXYZ2RotationMatrix([0.3, -0.7, 1.1]), what='HT(Rxyz)')
Check(exu.HT(rotationVector=[0.2, 0.5, -0.1]).rotation, RotationVector2RotationMatrix([0.2, 0.5, -0.1]), what='HT(rotationVector)')

#the coordinates of the nodes, both ways
Check(H.GetCoordinatesEP(), list(p) + list(RotationMatrix2EulerParameters(A)), what='GetCoordinatesEP')
for (get, put) in [('GetCoordinatesEP', 'SetCoordinatesEP'), ('GetCoordinatesRxyz', 'SetCoordinatesRxyz'),
                   ('GetCoordinatesRotationVector', 'SetCoordinatesRotationVector')]:
    H2 = getattr(exu.HT(), put)(getattr(H, get)())
    Check(H2.HT44(), H.HT44(), what=put)

#the same reference coordinates as CreateRigidBody gives its node
SC = exu.SystemContainer()
mbs = SC.AddSystem()
for (nodeType, get) in [(exu.NodeType.RotationEulerParameters, 'GetCoordinatesEP'),
                        (exu.NodeType.RotationRxyz, 'GetCoordinatesRxyz'),
                        (exu.NodeType.RotationRotationVector, 'GetCoordinatesRotationVector')]:
    body = mbs.CreateRigidBody(inertia=InertiaCuboid(1000, [0.1, 0.2, 0.3]), referenceHT=H, nodeType=nodeType,
                               returnDict=True)
    Check(mbs.GetNodeParameter(body['nodeNumber'], 'referenceCoordinates'), getattr(H, get)(), 1e-12, 'CreateRigidBody ' + get)

#Relative and the interpolations
H1 = exu.HT(Rxyz=[-0.2, 0.4, 0.9], translation=[1, 2, -0.5])
Check(H.Relative(H1).HT44(), (H.Inverse() * H1).HT44(), what='Relative')
for interpolate in ['InterpolateSO3', 'InterpolateSE3']:
    Check(getattr(H, interpolate)(H1, 0.).HT44(), H.HT44(), what=interpolate + ' 0')
    Check(getattr(H, interpolate)(H1, 1.).HT44(), H1.HT44(), 1e-12, interpolate + ' 1')
Hhalf = H.InterpolateSO3(H1, 0.5)
Check(Hhalf.translation, 0.5 * (p + H1.translation), what='InterpolateSO3 translation')
Check(H.Relative(Hhalf).RotationAngle(), 0.5 * H.Relative(H1).RotationAngle(), what='InterpolateSO3 angle')
HhalfSE3 = H.InterpolateSE3(H1, 0.5)
Check((H * H.Relative(HhalfSE3) * H.Relative(HhalfSE3)).HT44(), H1.HT44(), 1e-12, 'InterpolateSE3 halfway') #constant twist: two half steps

#the logarithm and the exponential map on SE(3) and R3xSO(3), against lieGroupBasics (#2819)
from exudyn.lieGroupBasics import ExpSE3, LogSE3, Skew2Vec
v = H.LogSE3()
logH = LogSE3(H.HT44())                                       #4x4: the skew matrix of Omega and U
Check(v, list(logH[0:3, 3]) + list(Skew2Vec(logH[0:3, 0:3])), what='LogSE3')
Check(exu.HT().SetExpSE3(v).HT44(), H.HT44(), what='SetExpSE3(LogSE3())')
Check(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')
Check((H * exu.HT().SetExpSE3(0.5 * H.Relative(H1).LogSE3())).HT44(), HhalfSE3.HT44(), what='SetExpSE3 as InterpolateSE3')
Check(H.LogR3xSO3(), H.GetCoordinatesRotationVector(), what='LogR3xSO3')
Check(exu.HT().SetExpR3xSO3(H.LogR3xSO3()).HT44(), H.HT44(), what='SetExpR3xSO3(LogR3xSO3())')

#angle and axis
H3 = exu.HT().SetRotationAxis([0, 3, 4], 0.8) #the Set functions return the HT
Check(H3.RotationAngle(), 0.8, what='RotationAngle')
Check(H3.RotationAxis(), [0, 0.6, 0.8], what='RotationAxis')
Check(exu.HT().RotationAxis(raiseError=False), [0, 0, 0], what='RotationAxis of no rotation')
if hasRangeChecks:
    try:
        exu.HT().RotationAxis()
        errors += 1 #must raise
    except Exception:
        pass

#a copy of an HT (#2824)
H5 = exu.HT(H)
H5.translation = [0, 0, 0]
Check(H.translation, p, what='HT(H) is a copy')
Check(H5.rotation, A, what='HT(H) rotation')

#numpy indexing, @ and copies, as for a 4x4 numpy array (#2821)
import copy
Check(H[0:3,3], p, what='H[0:3,3]')
Check(H[2][3], p[2], what='H[2][3]')
H6 = exu.HT(H)
H6[0:3,3] = [1, 2, 3]
Check(H6.translation, [1, 2, 3], what='H[0:3,3] = value')
if hasRangeChecks:
    try:
        H6[3,3] = 2
        errors += 1 #the last row must raise
    except Exception:
        pass
Check((H @ H1).HT44(), H.HT44() @ H1.HT44(), what='H@H1')
Check(H @ [0.1, 0.2, 0.3, 1], H.HT44() @ [0.1, 0.2, 0.3, 1], what='H@x')
Check(copy.deepcopy([H])[0].HT44(), H.HT44(), what='deepcopy')

#chained Set functions, keywords that keep the other part, HasNoRotation
H4 = exu.HT().SetRotationZ(0.5).Set(translation=[1, 2, 3])
Check(H4.HT44(), HTtranslate([1, 2, 3]) @ HTrotateZ(0.5), what='chained Set')
if exu.HT(rotation=np.eye(3), translation=[1, 0, 0]).HasNoRotation() is not True or H4.HasNoRotation():
    errors += 1
if hasRangeChecks:
    try:
        exu.HT(Rxyz=[0, 0, 1], rotationVector=[0, 0, 1])
        errors += 1 #two rotations must raise
    except Exception:
        pass

exu.Print('homogeneousTransformationInterfaceTest: errors', errors)
u = errors + np.sum(H.GetCoordinatesEP()) + np.sum(HhalfSE3.HT44()) + H3.RotationAngle()
exu.Print('solution of homogeneousTransformationInterfaceTest=', u)

exu.sys['testResult'] = u
```