coordinateVectorConstraintGenericODE2.py
You can view and download this file on Github: coordinateVectorConstraintGenericODE2.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Example of double pendulum with Mass points: CoordinateVectorConstraint and GenericODE2
5#
6# Author: Johannes Gerstmayr
7# Date: 2022-03-17
8#
9# 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.
10#
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13import exudyn as exu
14from exudyn.utilities import *
15import numpy as np
16
17useGraphics = True #without test
18#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
20try: #only if called from test suite
21 from modelUnitTests import exudynTestGlobals #for globally storing test results
22 useGraphics = exudynTestGlobals.useGraphics
23except:
24 class ExudynTestGlobals:
25 pass
26 exudynTestGlobals = ExudynTestGlobals()
27#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
29SC = exu.SystemContainer()
30mbs = SC.AddSystem()
31
32withUserFunction = True
33
34L = 0.8 #length of arm
35mass = 2.5
36g = 9.81
37
38r = 0.05 #just for graphics
39d = r/2
40
41#add ground object and mass point:
42sizeRect = 1.2*L*2
43#graphicsBackground = GraphicsDataRectangle(-sizeRect,-sizeRect, sizeRect, 0.2*L, [1,1,1,1]) #for appropriate zoom
44graphicsBackground = GraphicsDataCheckerBoard(point=[0,-0.5*sizeRect,-2*r],size=sizeRect*1.8)
45
46oGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
47 visualization = VObjectGround(graphicsData = [graphicsBackground])))
48
49
50graphicsSphere = GraphicsDataSphere(point=[0,0,0], radius=r, color=color4steelblue, nTiles = 16)
51
52nR0 = mbs.AddNode(Point2D(referenceCoordinates=[L,0]))
53
54mGround0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition = [0,0,0]))
55mTip0 = mbs.AddMarker(MarkerNodePosition(nodeNumber=nR0))
56
57if not withUserFunction: #with internal terms:
58 oCD0 = mbs.AddObject(DistanceConstraint(markerNumbers=[mGround0, mTip0], distance=L))
59else:
60 #just for drawing, with inactive connector:
61 oCD0 = mbs.AddObject(DistanceConstraint(markerNumbers=[mGround0, mTip0], distance=L, activeConnector=False))
62
63#
64mbs.AddLoad(Force(markerNumber = mTip0, loadVector = [0, -mass*g, 0]))
65
66fileNameDouble = 'solution/coordVecConstraintRefDouble.txt'
67
68sPos0 = mbs.AddSensor(SensorNode(nodeNumber = nR0, storeInternal = True,
69 outputVariableType=exu.OutputVariableType.Position))
70
71
72graphicsSphere = GraphicsDataSphere(point=[0,0,0], radius=r, color=color4red, nTiles = 16)
73nR1 = mbs.AddNode(Point2D(referenceCoordinates=[L*2,0]))
74
75#instead of MassPoint2D, create one object ...
76oGeneric = mbs.AddObject(ObjectGenericODE2(nodeNumbers=[nR0, nR1],
77 massMatrix=mass*np.eye(4)))
78
79mTip1 = mbs.AddMarker(MarkerNodePosition(nodeNumber=nR1))
80
81if not withUserFunction: #with internal terms:
82 oCD1 = mbs.AddObject(DistanceConstraint(markerNumbers=[mTip0, mTip1], distance=L))
83else:
84 #just for drawing, with inactive connector:
85 mbs.AddObject(DistanceConstraint(markerNumbers=[mTip0, mTip1], distance=L, activeConnector=False))
86
87 nGround = mbs.AddNode(NodePointGround())
88 mCoordsGround = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nGround))
89
90 mCoords0 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nR0))
91 mCoords1 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nR1))
92
93 mCoordsAll = mbs.AddMarker(MarkerObjectODE2Coordinates(objectNumber=oGeneric))
94
95 def UFconstraint(mbs, t, itemNumber, q, q_t,velocityLevel):
96 #print("q=", q, ", q_t=", q_t)
97 return [np.sqrt(q[0]**2 + q[1]**2) - L,
98 np.sqrt((q[2]-q[0])**2 + (q[3]-q[1])**2) - L]
99
100 def UFjacobian(mbs, t, itemNumber, q, q_t,velocityLevel):
101 #print("q=", q, ", q_t=", q_t)
102 jac = np.zeros((2,4))
103
104 f0 = np.sqrt(q[0]**2 + q[1]**2)
105 jac[0,0] = q[0]/f0
106 jac[0,1] = q[1]/f0
107
108 f1 = np.sqrt((q[2]-q[0])**2 + (q[3]-q[1])**2)
109 jac[1,0] =-(q[2]-q[0])/f1
110 jac[1,1] =-(q[3]-q[1])/f1
111 jac[1,2] = (q[2]-q[0])/f1
112 jac[1,3] = (q[3]-q[1])/f1
113 return jac
114
115
116 mbs.AddObject(CoordinateVectorConstraint(markerNumbers=[mCoordsGround, mCoordsAll],
117 #markerNumbers=[mCoords0, mCoords1], #ALTERNATIVELY: with markers on nodes (but only works for max. 2 nodes!)
118 scalingMarker0=np.zeros((2,4)), #needed to define number of algebraic equations; rows=nAE, cols=len(q) of mCoordsGround + mCoords0
119 constraintUserFunction=UFconstraint,
120 jacobianUserFunction=UFjacobian,
121 visualization=VCoordinateVectorConstraint(show=False)))
122
123
124#q
125mbs.AddLoad(Force(markerNumber = mTip1, loadVector = [0, -mass*g, 0]))
126
127sPos1 = mbs.AddSensor(SensorNode(nodeNumber = nR1, storeInternal = True,
128 #fileName=fileNameDouble,
129 outputVariableType=exu.OutputVariableType.Position))
130
131
132
133mbs.Assemble()
134
135simulationSettings = exu.SimulationSettings()
136
137#useGraphics=False
138tEnd = 1
139h = 1e-3
140if useGraphics:
141 tEnd = 1
142 simulationSettings.timeIntegration.simulateInRealtime = True
143 simulationSettings.timeIntegration.realtimeFactor = 1
144
145simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
146simulationSettings.timeIntegration.endTime = tEnd
147
148#simulationSettings.solutionSettings.solutionWritePeriod = h
149simulationSettings.timeIntegration.verboseMode = 1
150#simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
151
152simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8 #SHOULD work with 0.9 as well
153
154SC.visualizationSettings.nodes.showBasis=True
155SC.visualizationSettings.nodes.drawNodesAsPoint=False
156SC.visualizationSettings.nodes.defaultSize=r
157
158if useGraphics:
159 exu.StartRenderer()
160 mbs.WaitForUserToContinue()
161
162mbs.SolveDynamic(simulationSettings)
163
164p0=mbs.GetNodeOutput(nR0, exu.OutputVariableType.Position)
165exu.Print("p0=", list(p0))
166u=sum(p0)
167
168exu.Print('solution of coordinateVectorConstraint=',u)
169
170exudynTestGlobals.testError = u - (-1.0825265797698322)
171exudynTestGlobals.testResult = u
172
173
174#%%++++++++++++++++++++++++++++
175if useGraphics:
176 SC.WaitForRenderEngineStopFlag()
177 exu.StopRenderer() #safely close rendering window!
178
179 from exudyn.plot import PlotSensorDefaults
180 PlotSensorDefaults().fontSize = 12
181 # PlotSensorDefaults().markerStyles=['x','o ','v ','^ ','s ']
182 # mbs.PlotSensor([sPos0,sPos0,sPos1,sPos1], components=[0,1,0,1], closeAll=True)
183
184 #if reference solution computed:
185 mbs.PlotSensor([sPos0,sPos0,sPos1,sPos1,fileNameDouble], components=[0,1,0,1,1], closeAll=True,
186 markerStyles=['','','','','x'], lineStyles=['-','-','-','-',''])