<!-- GENERATED by tools/generators/examplesDocsEmitter.py from python/ - do not edit -->
(examples-homogeneoustransformationinterpolation)=
# homogeneousTransformationInterpolation.py

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

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

#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details:  Four rigid bodies driven along homogeneous transformations of exudyn.HT. Each body is
#           fixed by a generic joint, all axes constrained, to a ground object, whose frame
#           (referenceHT) a PreStepUserFunction sets at every step; the grounds only prescribe the
#           motion and are not drawn. Each pair of bodies goes from the same start frame H0 to the
#           same end frame H1: one with InterpolateSO3 (the translation along a straight line, the
#           rotation about one fixed axis), the other with InterpolateSE3 (a screw motion, translation
#           and rotation coupled). The first pair turns by a quarter about z, the second by a half turn
#           about a skew axis. The solution is stored every 5 ms and shown with the SolutionViewer,
#           forward and backward.
#
# Author:   Johannes Gerstmayr
# Date:     2026-10-04
#
# 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 InertiaCuboid
import exudyn.graphics as graphics
import numpy as np

SC = exu.SystemContainer()
mbs = SC.AddSystem()

tEnd = 4     #time for going from the start to the end frame
sizes = [[0.8, 0.4, 0.2],
         [0.79, 0.39, 0.195]]

#the start and end frames of the two pairs; the Set functions return the HT
pairs = [(exu.HT(translation=[-2, 1., 0]),
          exu.HT(Rxyz=[0, 0, 0.5*np.pi], translation=[2, 1.5, 0])),             #a quarter turn about z
         (exu.HT(translation=[-2, -1., 0]),
          exu.HT().SetRotationAxis([1, 1, 1], np.pi).Set(translation=[2, -1.5, 0]))] #a half turn about a skew axis

oGroundCB = mbs.CreateGround(graphicsDataList=[graphics.CheckerBoard([0,0,-1],
                                                                     size=8)])

#each body is fixed to a ground object; the ground is moved, the body follows through the joint
drivers = [] #(ground, body, H0, H1, interpolation)
for (H0, H1) in pairs:
    cnt = 0 #for different sizes to avoid graphics artifacts
    for (interpolation, color) in [('InterpolateSO3', graphics.color.red), ('InterpolateSE3', graphics.color.blue)]:
        oGround = mbs.CreateGround(referenceHT=H0)
        oBody = mbs.CreateRigidBody(inertia=InertiaCuboid(density=1000, sideLengths=sizes[cnt]),
                                    referenceHT=H0,
                                    graphicsDataList=[graphics.Brick(size=sizes[cnt], color=color),
                                                      graphics.Basis(length=0.5, radius=0.01)])
        mbs.CreateGenericJoint(itemNumbers=[oGround, oBody], position=H0.translation,
                               constrainedAxes=[1,1,1, 1,1,1], show=False)
        drivers += [(oGround, oBody, H0, H1, interpolation)]
        cnt += 1

def PreStepUserFunction(mbs, t):
    factor = 0.5 - 0.5*np.cos(np.pi*min(t/tEnd, 1.)) #from 0 to 1, starting and ending at rest
    for (oGround, oBody, H0, H1, interpolation) in drivers:
        mbs.SetObjectParameter(oGround, 'referenceHT', getattr(H0, interpolation)(H1, factor))
    return True

mbs.SetPreStepUserFunction(PreStepUserFunction)
mbs.Assemble()

simulationSettings = exu.SimulationSettings()
simulationSettings.timeIntegration.numberOfSteps = 800
simulationSettings.timeIntegration.endTime = tEnd
simulationSettings.solution.file.writePeriod = 0.01

SC.visualizationSettings.general.renderWindowString = ('H(t) = H0.InterpolateSO3(H1, f(t)) (red), H0.InterpolateSE3(H1, f(t)) (blue)\n'
                                                       'f(t) = (1 - cos(pi*t/tEnd))/2: from H0 at t=0 to H1 at t=tEnd')
SC.visualizationSettings.view0.window.renderWindowSize = [1200, 800]
SC.visualizationSettings.openGL.light0.shadow = 0.3
SC.visualizationSettings.openGL.light0.position = [2,2,10,1]

mbs.SolveDynamic(simulationSettings)

SC.renderer.Start()
SC.renderer.SetModelView(zoom=2.386449,
                         rotationVector=[-0.7689536,0.05280677,0.2063775],
                         centerPoint=[0.04670653,0.02902222,0])
                       
mbs.SolutionViewer(timeout=0.01) #the motion, forward and backward

#at the end, both bodies of a pair are in the end frame
error = 0
for (oGround, oBody, H0, H1, interpolation) in drivers:
    H = mbs.GetObjectOutputBody(oBody, exu.OutputVariableType.HomogeneousTransformation, localPosition=[0,0,0])
    error += np.linalg.norm(H.HT44() - H1.HT44())
exu.Print('homogeneousTransformationInterpolation: distance of the bodies to the end frames =', error)
```