coordinateVectorConstraint.py

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

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