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