homogeneousTransformationTest.py

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

 1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2# This is an EXUDYN example
 3#
 4# Details:  exudyn.HT, the homogeneous transformation of Exudyn's C++ core (#2780), against the 4x4 numpy matrices
 5#           of exudyn.rigidBodyUtilities: for random rotations and translations, the composition H1*H2, the
 6#           transformed point H*v, the inverse, the 4x4 matrix in both directions, and the transformations set
 7#           without rotation (identity, SetTranslation), whose products skip the rotation. The output variable
 8#           HomogeneousTransformation of a node, a body point, a marker and a sensor: an exu.HT of the rotation matrix
 9#           and the position; a sensor stores its 16 values row by row, exu.HT(values).
10#
11# Author:   Johannes Gerstmayr
12# Date:     2026-10-02
13#
14# 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.
15#
16#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
18import exudyn as exu
19from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
20import numpy as np
21
22testIsActive = exu.sys.get('testIsActive', False)
23
24rng = np.random.default_rng(1)
25errors = []
26total = 0.
27for i in range(20):
28    A1 = RotXYZ2RotationMatrix(rng.standard_normal(3))
29    A2 = RotXYZ2RotationMatrix(rng.standard_normal(3))
30    p1 = rng.standard_normal(3)
31    p2 = rng.standard_normal(3)
32    v = rng.standard_normal(3)
33    H1 = exu.HT(rotation=A1, translation=p1)
34    H2 = exu.HT(rotation=A2, translation=p2)
35    T1 = HomogeneousTransformation(A1, p1) #4x4 numpy matrices
36    T2 = HomogeneousTransformation(A2, p2)
37    errors += [np.abs((H1*H2).HT44() - T1 @ T2).max(),                       #composition
38               np.abs(H1*v - (T1 @ np.append(v, 1))[0:3]).max(),            #transformed point
39               np.abs(H1.Inverse().HT44() - InverseHT(T1)).max(),             #inverse
40               np.abs(exu.HT(T1).HT44() - T1).max(),                          #from and to 4x4
41               np.abs(H1.RotateVectorTransposed(H1.RotateVector(v)) - v).max()]
42    total += np.abs((H1*H2).HT44()).sum()
43
44#transformations without rotation: the flag, and the same results as with a unit matrix
45HT0 = exu.HT()
46Htranslation = exu.HT(translation=[1, 2, 3])
47Hrotation = exu.HT(rotation=RotationMatrixZ(0.3))
48exu.Print('without rotation:', HT0.HasNoRotation(), Htranslation.HasNoRotation(), Hrotation.HasNoRotation(),
49          (Htranslation*HT0).HasNoRotation(), (Htranslation*Hrotation).HasNoRotation())
50errors += [np.abs((Htranslation*Hrotation).HT44() - HTtranslate([1, 2, 3]) @ HTrotateZ(0.3)).max(),
51           np.abs((Hrotation*Htranslation).HT44() - HTrotateZ(0.3) @ HTtranslate([1, 2, 3])).max(),
52           np.abs(Htranslation.Inverse().HT44() - HTtranslate([-1, -2, -3])).max()]
53flags = [HT0.HasNoRotation(), Htranslation.HasNoRotation(), not Hrotation.HasNoRotation(),
54         (Htranslation*HT0).HasNoRotation(), not (Htranslation*Hrotation).HasNoRotation()]
55
56#the output variable HomogeneousTransformation of a rotating rigid body, after a few steps
57SC = exu.SystemContainer()
58mbs = SC.AddSystem()
59body = mbs.CreateRigidBody(inertia=InertiaCuboid(1000, [0.1, 0.2, 0.3]), referencePosition=[1, 2, 3],
60                           referenceRotationMatrix=RotationMatrixZ(0.4), initialAngularVelocity=[0.3, 0.2, 1],
61                           initialVelocity=[0.1, 0, 0], returnDict=True)
62pLocal = [0.1, 0.05, 0]
63marker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=body['bodyNumber'], localPosition=pLocal))
64OVHT = exu.OutputVariableType.HomogeneousTransformation
65sensor = mbs.AddSensor(SensorBody(bodyNumber=body['bodyNumber'], localPosition=pLocal, outputVariableType=OVHT, storeInternal=True))
66mbs.Assemble()
67simulationSettings = exu.SimulationSettings()
68simulationSettings.timeIntegration.newton.useModifiedNewton = False #Just for the test; modified Newton is usually faster
69simulationSettings.timeIntegration.numberOfSteps = 100
70simulationSettings.timeIntegration.endTime = 0.5
71simulationSettings.solution.file.write = False
72mbs.SolveDynamic(simulationSettings)
73
74A = mbs.GetObjectOutputBody(body['bodyNumber'], exu.OutputVariableType.RotationMatrix, localPosition=pLocal).reshape(3, 3)
75p = mbs.GetObjectOutputBody(body['bodyNumber'], exu.OutputVariableType.Position, localPosition=pLocal)
76HTbody = mbs.GetObjectOutputBody(body['bodyNumber'], OVHT, localPosition=pLocal) #an exu.HT
77errors += [np.abs(HTbody.HT44() - HomogeneousTransformation(A, p)).max(),
78           np.abs(mbs.GetMarkerOutput(marker, OVHT).HT44() - HTbody.HT44()).max(),
79           np.abs(exu.HT(mbs.GetSensorValues(sensor)).HT44() - HTbody.HT44()).max(),
80           np.abs(mbs.GetNodeOutput(body['nodeNumber'], OVHT) * pLocal - p).max()]
81total += np.abs(HTbody.HT44()).sum()
82
83exu.Print('largest difference to rigidBodyUtilities:', max(errors), ', flags as expected:', all(flags))
84testResult = total + (max(errors) < 1e-12) + sum(flags)
85exu.Print('solution of homogeneousTransformationTest=', testResult)
86exu.sys['testResult'] = testResult