kinematicTreePrismaticJacobianTest.py

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

 1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2# This is an EXUDYN example
 3#
 4# Details:  The position Jacobian of a prismatic joint of ObjectKinematicTree in a rotated joint frame,
 5#           tested through a force on a MarkerKinematicTreeRigid (#2740). A prismatic link is rotated
 6#           either by a revolute parent link or by its joint transformation; a force of 1 N along
 7#           the prismatic axis accelerates a mass of 2 kg by 0.5 m/s^2, so q1 = 0.25 after 1 s in
 8#           every case - as long as the Jacobian column is the global axis.
 9#
10# Author:   Johannes Gerstmayr
11# Date:     2026-09-29
12#
13# 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.
14#
15#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
17import exudyn as exu
18from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
19
20import numpy as np
21
22testIsActive = exu.sys.get('testIsActive', False)
23
24SC = exu.SystemContainer()
25mbs = SC.AddSystem()
26
27mass = 2        #mass of the prismatic link
28force = 1       #force along the prismatic axis
29testResult = 0
30
31#(angle of the revolute parent link, rotation of the prismatic joint frame about z)
32cases = [(0, 0), (0.25*np.pi, 0), (0.5*np.pi, 0), (0, 0.5*np.pi), (0.5*np.pi, 0.25*np.pi)]
33for (q0, alpha) in cases:
34    mbs.Reset()
35    nGeneric = mbs.AddNode(NodeGenericODE2(referenceCoordinates=[0.,0.], initialCoordinates=[q0,0.],
36                                           initialCoordinates_t=[0.,0.], numberOfODE2Coordinates=2))
37    #link 0: revolute about z, held by a huge inertia; link 1: prismatic along the x-axis of its joint frame
38    oKT = mbs.AddObject(ObjectKinematicTree(nodeNumber=nGeneric,
39                                            jointTypes=[exu.JointType.RevoluteZ, exu.JointType.PrismaticX],
40                                            linkParents=[-1, 0],
41                                            jointTransformations=exu.Matrix3DList([np.eye(3), RotationMatrixZ(alpha)]),
42                                            jointOffsets=exu.Vector3DList([[0,0,0], [0,0,0]]),
43                                            linkInertiasCOM=exu.Matrix3DList([1e8*np.eye(3), np.eye(3)]),
44                                            linkCOMs=exu.Vector3DList([[0,0,0], [0,0,0]]),
45                                            linkMasses=[1e8, mass]))
46    mLink1 = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=1, localPosition=[0,0,0]))
47    #the prismatic axis in the global frame; its line passes through the revolute axis, so no torque on link 0
48    axis = RotationMatrixZ(q0 + alpha) @ np.array([1,0,0])
49    mbs.AddLoad(LoadForceVector(markerNumber=mLink1, loadVector=force*axis))
50
51    mbs.Assemble()
52    simulationSettings = exu.SimulationSettings()
53    simulationSettings.timeIntegration.numberOfSteps = 100
54    simulationSettings.timeIntegration.endTime = 1
55    simulationSettings.solution.file.write = not testIsActive
56    mbs.SolveDynamic(simulationSettings)
57
58    q = mbs.GetNodeOutput(nGeneric, exu.OutputVariableType.Coordinates)
59    exu.Print('q0 =', round(q0, 4), ', alpha =', round(alpha, 4), ': q1 =', q[1], '(expected 0.25)')
60    testResult += q[1]
61
62#5 cases of 0.25
63exu.Print('solution of kinematicTreePrismaticJacobianTest=', testResult)
64exu.sys['testResult'] = testResult