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