kinematicTreeUserFunctionTest.py

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

 1#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2# This is an EXUDYN example
 3#
 4# Details:  The forceUserFunction of ObjectKinematicTree, which receives the joint coordinates q and velocities q_t
 5#           (#2845): two equal trees of a revolute and a prismatic joint under gravity, the first damped by the D control
 6#           of the tree (jointDControlVector), the second by a user function that returns the same forces from q_t;
 7#           both must move alike.
 8#
 9# Author:   Johannes Gerstmayr
10# Date:     2026-10-04
11#
12# 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.
13#
14#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
16import exudyn as exu
17from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
18import numpy as np
19
20testIsActive = exu.sys.get('testIsActive', False)
21
22damping = [0.8, 5.]
23
24def Tree(mbs, userFunction=0):
25    """a revolute joint about z with a link along x, then a prismatic joint along x, under gravity in -y"""
26    nKT = mbs.AddNode(NodeGenericODE2(referenceCoordinates=[0, 0.2], initialCoordinates=[0.3, 0], initialCoordinates_t=[0, 0.1],
27                                      numberOfODE2Coordinates=2))
28    useControl = userFunction == 0
29    oKT = mbs.AddObject(ObjectKinematicTree(nodeNumber=nKT, linkParents=[-1, 0],
30                                            jointTypes=[exu.JointType.RevoluteZ, exu.JointType.PrismaticX],
31                                            jointTransformations=exu.Matrix3DList([np.eye(3)]*2),
32                                            jointOffsets=exu.Vector3DList([[0, 0, 0], [0.5, 0, 0]]),
33                                            linkInertiasCOM=exu.Matrix3DList([np.eye(3)*0.01]*2),
34                                            linkCOMs=exu.Vector3DList([[0.25, 0, 0], [0.1, 0, 0]]),
35                                            linkMasses=[1, 2], gravity=[0, -9.81, 0],
36                                            jointPositionOffsetVector=[0.1, 0.2], jointPControlVector=[50, 200],
37                                            jointVelocityOffsetVector=[0, 0] if useControl else [],
38                                            jointDControlVector=damping if useControl else [],
39                                            forceUserFunction=userFunction))
40    return nKT
41
42#the D control as a user function: the tree subtracts what it returns, as it subtracts D*(0 - q_t)
43def UFforce(mbs, t, itemNumber, q, q_t):
44    return [-damping[0]*q_t[0], -damping[1]*q_t[1]]
45
46SC = exu.SystemContainer()
47mbs = SC.AddSystem()
48nControl = Tree(mbs)
49nUser = Tree(mbs, UFforce)
50mbs.Assemble()
51
52simulationSettings = exu.SimulationSettings()
53simulationSettings.timeIntegration.numberOfSteps = 500
54simulationSettings.timeIntegration.endTime = 1
55simulationSettings.solution.file.write = False
56mbs.SolveDynamic(simulationSettings)
57
58qControl = mbs.GetNodeOutput(nControl, exu.OutputVariableType.Coordinates)
59qUser = mbs.GetNodeOutput(nUser, exu.OutputVariableType.Coordinates)
60exu.Print('q control=', qControl, ', q user function=', qUser)
61result = np.sum(qControl) + (1 if np.linalg.norm(qControl - qUser) > 1e-10 else 0) #the second part must be zero
62
63exu.Print('kinematicTreeUserFunctionTest result=', result)
64exu.sys['testResult'] = result