connectorFrameOutputsTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  The frame outputs of joints and connectors (#2870): HomogeneousTransformation is the joint frame J0,
  5#           HomogeneousTransformationLocal the joint frame J1 relative to J0, Displacement the global vector between
  6#           the markers. Three rigid bodies on a revolute, a generic and a prismatic joint, the last one with a linear
  7#           spring-damper, and a fourth on a rigid-body spring-damper move under gravity; then J0 times the local
  8#           frame must give J1 as the markers and the rotationMarker1 of the joint say, the translation of the local
  9#           frame must equal DisplacementLocal, and a SensorObject stores the 16 values. The 2D joints: a pendulum on
 10#           JointRevolute2D (Position, Force), a body on JointPrismatic2D (ForceLocal, DisplacementLocal against
 11#           Distance), and a body on a cable with JointSliding2D (DisplacementLocal, its drift; SlidingCoordinate, #2872).
 12#
 13# Author:   Johannes Gerstmayr
 14# Date:     2026-10-06
 15#
 16# 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.
 17#
 18#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 19
 20import exudyn as exu
 21from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 22import numpy as np
 23
 24testIsActive = exu.sys.get('testIsActive', False)
 25
 26OVT = exu.OutputVariableType
 27g = 9.81
 28inertia = InertiaCuboid(density=1000, sideLengths=[0.2,0.1,0.1])
 29checks = [] #(name, passed)
 30
 31def Check(name, passed):
 32    checks.append((name, bool(passed)))
 33    if not passed:
 34        exu.Print('connectorFrameOutputsTest: FAILED', name)
 35
 36def Settings(endTime, steps):
 37    simulationSettings = exu.SimulationSettings()
 38    simulationSettings.solution.file.write = False
 39    simulationSettings.timeIntegration.verboseMode = 0
 40    simulationSettings.timeIntegration.numberOfSteps = steps
 41    simulationSettings.timeIntegration.endTime = endTime
 42    simulationSettings.timeIntegration.newton.useModifiedNewton = False
 43    return simulationSettings
 44
 45#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 46#the 3D joints and the two rigid-body connectors
 47SC = exu.SystemContainer()
 48mbs = SC.AddSystem()
 49oGround = mbs.CreateGround()
 50gravity = [0,-g,0]
 51oRevolute = mbs.CreateRigidBody(inertia=inertia, referencePosition=[1,0,0], gravity=gravity)
 52jRevolute = mbs.CreateRevoluteJoint(itemNumbers=[oGround, oRevolute], position=[0.5,0,0], axis=[0,0.2,1])
 53oGeneric = mbs.CreateRigidBody(inertia=inertia, referencePosition=[1,2,0], gravity=gravity,
 54                               referenceRotationMatrix=RotationMatrixX(0.4))
 55jGeneric = mbs.CreateGenericJoint(itemNumbers=[oGround, oGeneric], position=[0.5,2,0], constrainedAxes=[1,1,1,1,0,0],
 56                                  rotationMatrixAxes=RotationMatrixZ(0.3))
 57oPrismatic = mbs.CreateRigidBody(inertia=inertia, referencePosition=[1,4,0], gravity=gravity)
 58jPrismatic = mbs.CreatePrismaticJoint(itemNumbers=[oGround, oPrismatic], position=[1,4,0], axis=[1,-1,0])
 59cLinear = mbs.CreateLinearSpringDamper(itemNumbers=[oGround, oPrismatic], position=[1,4,0], axis=[1,-1,0],
 60                                       stiffness=2e3, damping=10)
 61oSpring = mbs.CreateRigidBody(inertia=inertia, referencePosition=[1,6,0], gravity=gravity,
 62                              initialAngularVelocity=[1,2,3])
 63cRigid = mbs.CreateRigidBodySpringDamper(itemNumbers=[oGround, oSpring], localPosition0=[1,6.1,0], localPosition1=[0,0.1,0],
 64                                         stiffness=np.diag([2e3,2e3,2e3,50,50,50]), damping=np.diag([5,5,5,0.5,0.5,0.5]),
 65                                         rotationMatrixJoint=RotationMatrixY(0.2))
 66sJointHT = mbs.AddSensor(SensorObject(objectNumber=jRevolute, storeInternal=True, outputVariableType=OVT.HomogeneousTransformationLocal))
 67
 68mbs.Assemble()
 69mbs.SolveDynamic(Settings(0.5, 500))
 70
 71def MarkerFrame(markerNumber, rotationMarker):
 72    """the frame of a marker times rotationMarker of the item"""
 73    H = mbs.GetMarkerOutput(markerNumber, OVT.HomogeneousTransformation).HT44()
 74    return HomogeneousTransformation(H[0:3,0:3] @ np.array(rotationMarker), H[0:3,3])
 75
 76for (name, item, rotationMarkers, hasHT) in [('JointRevoluteZ', jRevolute, True, True),
 77                                             ('JointGeneric', jGeneric, True, True),
 78                                             ('JointPrismaticX', jPrismatic, True, True),
 79                                             ('ConnectorRigidBodySpringDamper', cRigid, True, False),
 80                                             ('ConnectorLinearSpringDamper', cLinear, False, False)]:
 81    data = mbs.GetObject(item)
 82    [m0, m1] = data['markerNumbers']
 83    H0 = MarkerFrame(m0, data['rotationMarker0'] if rotationMarkers else np.eye(3))
 84    H1 = MarkerFrame(m1, data['rotationMarker1'] if rotationMarkers else np.eye(3))
 85    HLocal = mbs.GetObjectOutput(item, OVT.HomogeneousTransformationLocal).HT44()
 86    Check(name + ' local frame', np.abs(HLocal - np.linalg.inv(H0) @ H1).max() < 1e-12)
 87    Check(name + ' Displacement', np.abs(mbs.GetObjectOutput(item, OVT.Displacement) - (H1[0:3,3] - H0[0:3,3])).max() < 1e-12
 88          if name.startswith('Connector') else True)
 89    if hasHT:
 90        Check(name + ' frame J0', np.abs(mbs.GetObjectOutput(item, OVT.HomogeneousTransformation).HT44() - H0).max() < 1e-12)
 91        Check(name + ' DisplacementLocal', np.abs(HLocal[0:3,3] - mbs.GetObjectOutput(item, OVT.DisplacementLocal)).max() < 1e-12)
 92
 93#the revolute joint: the local frame turns about z by Rotation[2], up to the drift
 94HLocal = mbs.GetObjectOutput(jRevolute, OVT.HomogeneousTransformationLocal).HT44()
 95angle = mbs.GetObjectOutput(jRevolute, OVT.Rotation)[2]
 96Check('JointRevoluteZ turned', abs(angle) > 0.1 and np.abs(HLocal[0:3,0:3] - RotationMatrixZ(angle)).max() < 1e-8)
 97Check('sensor of the local frame', np.abs(mbs.GetSensorStoredData(sJointHT)[-1,1:] - HLocal.flatten()).max() < 1e-12)
 98
 99#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
100#the 2D joints at rest: the reaction force carries the weight; as for every joint, the force is the one on marker 0 (the ground)
101mass = 2.
102SC2 = exu.SystemContainer()
103mbs2 = SC2.AddSystem()
104oGround2 = mbs2.AddObject(ObjectGround())
105nPendulum = mbs2.AddNode(NodeRigidBody2D(referenceCoordinates=[0,-1,0]))
106oPendulum = mbs2.AddObject(ObjectRigidBody2D(nodeNumber=nPendulum, mass=mass, inertia=0.1))
107mbs2.AddLoad(LoadMassProportional(markerNumber=mbs2.AddMarker(MarkerBodyMass(bodyNumber=oPendulum)), loadVector=[0,-g,0]))
108mPendulumTop = mbs2.AddMarker(MarkerBodyPosition(bodyNumber=oPendulum, localPosition=[0,1,0]))
109jRevolute2D = mbs2.AddObject(ObjectJointRevolute2D(markerNumbers=[mbs2.AddMarker(MarkerBodyPosition(bodyNumber=oGround2)), mPendulumTop]))
110
111nSlider = mbs2.AddNode(NodeRigidBody2D(referenceCoordinates=[3,0,0], initialVelocities=[1,0,0]))
112oSlider = mbs2.AddObject(ObjectRigidBody2D(nodeNumber=nSlider, mass=mass, inertia=0.1))
113mbs2.AddLoad(LoadMassProportional(markerNumber=mbs2.AddMarker(MarkerBodyMass(bodyNumber=oSlider)), loadVector=[0,-g,0]))
114mGroundSlider = mbs2.AddMarker(MarkerBodyRigid(bodyNumber=oGround2, localPosition=[2,0,0]))
115jPrismatic2D = mbs2.AddObject(ObjectJointPrismatic2D(markerNumbers=[mGroundSlider, mbs2.AddMarker(MarkerBodyRigid(bodyNumber=oSlider))],
116                                                     axisMarker0=[2,0,0], normalMarker1=[0,1,0]))
117mbs2.Assemble()
118mbs2.SolveDynamic(Settings(0.2, 200))
119
120force = mbs2.GetObjectOutput(jRevolute2D, OVT.Force)
121Check('JointRevolute2D Force', abs(force[1] + mass*g) < 1e-8 and abs(force[0]) < 1e-8 and force[2] == 0)
122Check('JointRevolute2D Position', np.abs(mbs2.GetObjectOutput(jRevolute2D, OVT.Position)).max() == 0)
123Check('JointRevolute2D Velocity', np.abs(mbs2.GetObjectOutput(jRevolute2D, OVT.Velocity)).max() == 0)
124forceLocal = mbs2.GetObjectOutput(jPrismatic2D, OVT.ForceLocal)
125Check('JointPrismatic2D ForceLocal', abs(forceLocal[1] + mass*g) < 1e-8 and abs(forceLocal[0]) < 1e-8)
126displacementLocal = mbs2.GetObjectOutput(jPrismatic2D, OVT.DisplacementLocal)
127Check('JointPrismatic2D DisplacementLocal', abs(displacementLocal[0] - mbs2.GetObjectOutput(jPrismatic2D, OVT.Distance)) < 1e-12
128      and abs(displacementLocal[0] - 1.2) < 1e-8)
129Check('JointPrismatic2D frames', np.abs(mbs2.GetObjectOutput(jPrismatic2D, OVT.HomogeneousTransformationLocal).HT44()[0:3,3]
130                                        - displacementLocal).max() < 1e-12
131      and np.abs(mbs2.GetObjectOutput(jPrismatic2D, OVT.HomogeneousTransformation).HT44()[0:3,3] - [2,0,0]).max() == 0)
132
133#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
134#JointSliding2D: a body hanging from a cable, as in SlidingJoint2DTest; the sliding point stays on marker 0 up to the drift
135SC3 = exu.SystemContainer()
136mbs3 = SC3.AddSystem()
137L = 2; nElements = 3; lElem = L/nElements
138E = 2.07e11; rho = 7800; A = 1e-6; I = 1e-12/12
139nGround3 = mbs3.AddNode(NodePointGround())
140mGround3 = mbs3.AddMarker(MarkerNodeCoordinate(nodeNumber=nGround3, coordinate=0))
141cableList = []
142nc0 = mbs3.AddNode(Point2DS1(referenceCoordinates=[0,0,1,0]))
143for i in range(nElements):
144    mbs3.AddNode(Point2DS1(referenceCoordinates=[lElem*(i+1),0,1,0]))
145    cableList += [mbs3.AddObject(Cable2D(length=lElem, massPerLength=rho*A, bendingStiffness=E*I, axialStiffness=E*A,
146                                         nodeNumbers=[int(nc0)+i, int(nc0)+i+1]))]
147    mbs3.AddLoad(Gravity(markerNumber=mbs3.AddMarker(MarkerBodyMass(bodyNumber=cableList[-1])), loadVector=[0,-g,0]))
148for coordinate in [0,1,3]:
149    mbs3.AddObject(CoordinateConstraint(markerNumbers=[mGround3, mbs3.AddMarker(MarkerNodeCoordinate(nodeNumber=nc0, coordinate=coordinate))]))
150a = 0.1
151massRigid = 0.12
152nRigid = mbs3.AddNode(Rigid2D(referenceCoordinates=[lElem*1.5,-a,0]))
153oRigid = mbs3.AddObject(RigidBody2D(mass=massRigid, inertia=massRigid/12*(2*a)**2, nodeNumber=nRigid))
154markerRigidTop = mbs3.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[0.,a,0.]))
155mbs3.AddLoad(Force(markerNumber=mbs3.AddMarker(MarkerBodyPosition(bodyNumber=oRigid)), loadVector=[massRigid*g*0.1, -massRigid*g, 0]))
156cableMarkers = [mbs3.AddMarker(MarkerBodyCable2DCoordinates(bodyNumber=item)) for item in cableList]
157nodeData = mbs3.AddNode(NodeGenericData(initialCoordinates=[1, lElem*1.5], numberOfDataCoordinates=2))
158jSliding = mbs3.AddObject(ObjectJointSliding2D(markerNumbers=[markerRigidTop, cableMarkers[1]], slidingMarkerNumbers=cableMarkers,
159                                               slidingMarkerOffsets=[i*lElem for i in range(nElements)],
160                                               nodeNumber=nodeData, useClassicalFormulation=False))
161mbs3.Assemble()
162drift0 = mbs3.GetObjectOutput(jSliding, OVT.DisplacementLocal)
163settings3 = Settings(0.05, 50)
164settings3.timeIntegration.newton.relativeTolerance = 1e-6
165settings3.timeIntegration.newton.absoluteTolerance = 1e-8
166mbs3.SolveDynamic(settings3)
167drift = mbs3.GetObjectOutput(jSliding, OVT.DisplacementLocal)
168#the tangential component from the sliding coordinate of the step, counted once (#2872)
169Check('JointSliding2D DisplacementLocal', np.abs(drift0).max() < 1e-14 and abs(drift[0]) < 1e-8 and abs(drift[1]) < 1e-8
170      and drift[2] == 0)
171Check('JointSliding2D SlidingCoordinate', mbs3.GetObjectOutput(jSliding, OVT.SlidingCoordinate) == mbs3.GetNodeOutput(nodeData, OVT.Coordinates)[1])
172
173#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
174u = sum(int(passed) for (name, passed) in checks)
175exu.Print(u, 'of', len(checks), 'checks passed')
176exu.Print('solution of connectorFrameOutputsTest=', u)
177
178exu.sys['testResult'] = u