homogeneousTransformationInterpolation.py

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

 1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2# This is an EXUDYN example
 3#
 4# Details:  Four rigid bodies driven along homogeneous transformations of exudyn.HT. Each body is
 5#           fixed by a generic joint, all axes constrained, to a ground object, whose frame
 6#           (referenceHT) a PreStepUserFunction sets at every step; the grounds only prescribe the
 7#           motion and are not drawn. Each pair of bodies goes from the same start frame H0 to the
 8#           same end frame H1: one with InterpolateSO3 (the translation along a straight line, the
 9#           rotation about one fixed axis), the other with InterpolateSE3 (a screw motion, translation
10#           and rotation coupled). The first pair turns by a quarter about z, the second by a half turn
11#           about a skew axis. The solution is stored every 5 ms and shown with the SolutionViewer,
12#           forward and backward.
13#
14# Author:   Johannes Gerstmayr
15# Date:     2026-10-04
16#
17# 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.
18#
19#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
21import exudyn as exu
22from exudyn.utilities import InertiaCuboid
23import exudyn.graphics as graphics
24import numpy as np
25
26SC = exu.SystemContainer()
27mbs = SC.AddSystem()
28
29tEnd = 4     #time for going from the start to the end frame
30sizes = [[0.8, 0.4, 0.2],
31         [0.79, 0.39, 0.195]]
32
33#the start and end frames of the two pairs; the Set functions return the HT
34pairs = [(exu.HT(translation=[-2, 1., 0]),
35          exu.HT(Rxyz=[0, 0, 0.5*np.pi], translation=[2, 1.5, 0])),             #a quarter turn about z
36         (exu.HT(translation=[-2, -1., 0]),
37          exu.HT().SetRotationAxis([1, 1, 1], np.pi).Set(translation=[2, -1.5, 0]))] #a half turn about a skew axis
38
39oGroundCB = mbs.CreateGround(graphicsDataList=[graphics.CheckerBoard([0,0,-1],
40                                                                     size=8)])
41
42#each body is fixed to a ground object; the ground is moved, the body follows through the joint
43drivers = [] #(ground, body, H0, H1, interpolation)
44for (H0, H1) in pairs:
45    cnt = 0 #for different sizes to avoid graphics artifacts
46    for (interpolation, color) in [('InterpolateSO3', graphics.color.red), ('InterpolateSE3', graphics.color.blue)]:
47        oGround = mbs.CreateGround(referenceHT=H0)
48        oBody = mbs.CreateRigidBody(inertia=InertiaCuboid(density=1000, sideLengths=sizes[cnt]),
49                                    referenceHT=H0,
50                                    graphicsDataList=[graphics.Brick(size=sizes[cnt], color=color),
51                                                      graphics.Basis(length=0.5, radius=0.01)])
52        mbs.CreateGenericJoint(itemNumbers=[oGround, oBody], position=H0.translation,
53                               constrainedAxes=[1,1,1, 1,1,1], show=False)
54        drivers += [(oGround, oBody, H0, H1, interpolation)]
55        cnt += 1
56
57def PreStepUserFunction(mbs, t):
58    factor = 0.5 - 0.5*np.cos(np.pi*min(t/tEnd, 1.)) #from 0 to 1, starting and ending at rest
59    for (oGround, oBody, H0, H1, interpolation) in drivers:
60        mbs.SetObjectParameter(oGround, 'referenceHT', getattr(H0, interpolation)(H1, factor))
61    return True
62
63mbs.SetPreStepUserFunction(PreStepUserFunction)
64mbs.Assemble()
65
66simulationSettings = exu.SimulationSettings()
67simulationSettings.timeIntegration.numberOfSteps = 800
68simulationSettings.timeIntegration.endTime = tEnd
69simulationSettings.solution.file.writePeriod = 0.01
70
71SC.visualizationSettings.general.renderWindowString = ('H(t) = H0.InterpolateSO3(H1, f(t)) (red), H0.InterpolateSE3(H1, f(t)) (blue)\n'
72                                                       'f(t) = (1 - cos(pi*t/tEnd))/2: from H0 at t=0 to H1 at t=tEnd')
73SC.visualizationSettings.view0.window.renderWindowSize = [1200, 800]
74SC.visualizationSettings.openGL.light0.shadow = 0.3
75SC.visualizationSettings.openGL.light0.position = [2,2,10,1]
76
77mbs.SolveDynamic(simulationSettings)
78
79SC.renderer.Start()
80SC.renderer.SetModelView(zoom=2.386449,
81                         rotationVector=[-0.7689536,0.05280677,0.2063775],
82                         centerPoint=[0.04670653,0.02902222,0])
83                       
84mbs.SolutionViewer(timeout=0.01) #the motion, forward and backward
85
86#at the end, both bodies of a pair are in the end frame
87error = 0
88for (oGround, oBody, H0, H1, interpolation) in drivers:
89    H = mbs.GetObjectOutputBody(oBody, exu.OutputVariableType.HomogeneousTransformation, localPosition=[0,0,0])
90    error += np.linalg.norm(H.HT44() - H1.HT44())
91exu.Print('homogeneousTransformationInterpolation: distance of the bodies to the end frames =', error)