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

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

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

#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details:  Model of a four bar mechanism under gravity
#           Used as test case for CreateKinematicTree
#
# Author:   Johannes Gerstmayr
# Date:     2025-06-5
#
# 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 exudyn.graphics as graphics
import numpy as np

#import exudyn.rigidBodyUtilities as erb
#from exudyn.rigidBodyUtilities import HT0, HT2rotationMatrix, HT2translation

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


# create empty system
SC = exu.SystemContainer() # sc = systems container
mbs = SC.AddSystem() # mbs = multibodysystem


L1 = 0.8
L2 = 0.5
L3 = 0.3
w = 0.05

ground = mbs.CreateGround(referencePosition=[L2,L1-L3,0],
                          graphicsDataList=[graphics.CheckerBoard(size=4)])

#link with reference point at [0,0,0], COM at [0.5*L1,0,0]
link0Inertia = InertiaCuboid(density=1000, sideLengths=[L1,w,w]) #w.r.t. center of mass = [0,0,0]
link0Inertia = link0Inertia.Translated([0.5*L1,0,0]) #COM at [0.5*L1,0,0]


pdControl = (2e4,4e2)
#create tree links:
link0 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.RevoluteZ,
                 jointHT=exu.HT(translation=[0,0,0.5]),
                 PDcontrol=pdControl,
                 )
link1 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.RevoluteY,
                 jointHT=exu.HT(translation=[0.5,0,0.]),
                 PDcontrol=pdControl
                 )
#create kinematic tree:
oKT0 = mbs.CreateKinematicTree(
                              initialCoordinates=[pi,-0.5*pi],
                              listOfTreeLinks=[link0, link1],
                              colors=[graphics.color.blue,graphics.color.red],
                              gravity=[0,0,-9.81],
                              baseOffset=[1,0.5,0.],
                              linkRoundness=1,
                              )

#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#create tree links
link0 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.PrismaticX,
                 parent=-1,
                 jointHT=exu.HT(translation=[0,0,0.1]),
                 PDcontrol=(10*pdControl[0],10*pdControl[1]),
                 )

link1 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.RevoluteZ,
                 parent=0,
                 jointHT=exu.HT(translation=[-0.25,0,0.4]),
                 PDcontrol=pdControl,
                 )
link2 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.RevoluteZ,
                 parent=0,
                 jointHT=exu.HT(translation=[0.25,0,0.4]),
                 PDcontrol=pdControl,
                 )
link3 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.RevoluteX,
                 parent=1,
                 jointHT=exu.HT(translation=[0,0.5,0.]),
                 PDcontrol=pdControl
                 )
link4 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.RevoluteX,
                 parent=3,
                 jointHT=exu.HT(translation=[0,0.5,0.]),
                 PDcontrol=pdControl
                 )

link5 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.RevoluteX,
                 parent=2,
                 jointHT=exu.HT(translation=[0,0.5,0.]),
                 PDcontrol=pdControl
                 )
                 
link6 = TreeLink(linkInertia=link0Inertia,
                 jointType=exu.JointType.RevoluteX,
                 parent=5,
                 jointHT=exu.HT(translation=[0,0.5,0.]),
                 PDcontrol=pdControl
                 )

#create kinematic tree:
oKT1 = mbs.CreateKinematicTree(
                              initialCoordinates=[0.1, 0.125*pi,-0.125*pi]+[0]*4,
                              listOfTreeLinks=[link0, link1, link2, link3, link4, link5, link6],
                              colors=[graphics.color.orange]+[graphics.color.blue,graphics.color.red]*3,
                              #gravity=[0,0,-9.81],
                              baseOffset=[-1,-0.5,0.],
                              linkRoundness=0.5,
                              baseGraphicsDataList=[graphics.Cylinder(pAxis=[-0.5,0,0.05],vAxis=[1,0,0],radius=0.05,
                                                                   color=graphics.color.dodgerblue)]
                              )

def PreStepUserFunction(mbs, t):
    u = mbs.GetObjectParameter(oKT1,'jointPositionOffsetVector')
    u[0] = SmoothStep(t, 0.1, 0.2, 0, 0.25)
    u[1] = SmoothStep(t, 0.2, 0.3, 0, 0.25*pi)
    u[2] = SmoothStep(t, 0.2, 0.3, 0, -0.25*pi)
    u[3] = SmoothStep(t, 0.4, 0.5, 0, 0.5*pi)
    u[4] = SmoothStep(t, 0.6, 0.7, 0, 0.5*pi)
    u[5] = SmoothStep(t, 0.8, 0.9, 0, 0.5*pi)
    u[6] = SmoothStep(t, 1.0, 1.1, 0, 0.5*pi)
    mbs.SetObjectParameter(oKT1,'jointPositionOffsetVector',u)
    return True

mbs.SetPreStepUserFunction(PreStepUserFunction)


tEnd = 1.2
stepSize = 4e-3
mbs.Assemble()

simulationSettings = exu.SimulationSettings()
simulationSettings.timeIntegration.newton.useModifiedNewton = False #Just for the test; modified Newton is usually faster
simulationSettings.solution.file.write=(not testIsActive) #only the SolutionViewer reads it (#2492)
simulationSettings.solution.file.writePeriod=0.004
simulationSettings.timeIntegration.numberOfSteps = tEnd/stepSize
simulationSettings.timeIntegration.endTime = tEnd
simulationSettings.timeIntegration.verboseMode = 1
#simulationSettings.timeIntegration.realtime.active = True

SC.visualizationSettings.view0.scene.drawWorldBasis = True
SC.visualizationSettings.openGL.light0.shadow = 0.25
SC.visualizationSettings.openGL.multiSampling = 4
SC.visualizationSettings.view0.window.renderWindowSize=[1600,1200]

if not testIsActive:
    SC.renderer.Start()
    SC.renderer.RestoreSavedState()
    SC.renderer.DoIdleTasks()

mbs.SolveDynamic(simulationSettings)

if not testIsActive:
    SC.renderer.Stop()
    
    mbs.SolutionViewer()

#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
ode2 = mbs.systemData.GetODE2Coordinates()

u = np.linalg.norm(ode2)
exu.Print('solution of createKinematicTreeTest=',u) 

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