You can view and download this file on Github: reevingSystemOpen.py
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details: Example to calculate rope line of reeving system
#
# Author: Johannes Gerstmayr
# Date: 2022-02-02
#
# 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.
#
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
import exudyn as exu
from exudyn.itemInterface import *
from exudyn.utilities import *
from exudyn.beams import *
from exudyn.robotics import *
import numpy as np
from math import sin, cos, pi, sqrt , asin, acos, atan2
import copy
SC = exu.SystemContainer()
mbs = SC.AddSystem()
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#+++ MOTION PLANNING and TRAJECTORIES +++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#**function: Compute parameters for optimal trajectory using given duration and distance
#**notes: DEPRECATED, DO NOT USE - moved to robotics.motion
#**input: duration in seconds and distance in meters or radians
#**output: returns [vMax, accMax] with maximum velocity and maximum acceleration to achieve given trajectory
def ConstantAccelerationParameters(duration, distance):
accMax = 4*distance/duration**2
vMax = (accMax * distance)**0.5
return [vMax, accMax]
#**function: Compute angle / displacement s, velocity v and acceleration a
#**input:
# t: current time to compute values
# tStart: start time of profile
# sStart: start offset of path
# duration: duration of profile
# distance: total distance (of path) of profile
#**notes: DEPRECATED, DO NOT USE - moved to robotics.motion
#**output: [s, v, a] with path s, velocity v and acceleration a for constant acceleration profile; before tStart, solution is [0,0,0] while after duration, solution is [sStart+distance, 0, 0]
def ConstantAccelerationProfile(t, tStart, sStart, duration, distance):
[vMax, accMax] = ConstantAccelerationParameters(duration, distance)
s = sStart
v = 0
a = 0
x = t-tStart
if x < 0:
s=0
elif x < 0.5*duration:
s = sStart + 0.5*accMax*x**2
v = x*accMax
a = accMax
elif x < duration:
s = sStart + distance - 0.5*accMax * (duration-x)**2
v = (duration - x)*accMax
a = -accMax
else:
s = sStart + distance
return [s, v, a]
#**function: Compute joint value, velocity and acceleration for given robotTrajectory['PTP'] of point-to-point type, evaluated for current time t and joint number
#**input:
# t: time to evaluate trajectory
# robotTrajectory: dictionary to describe trajectory; in PTP case, either use 'time' points, or 'time' and 'duration', or 'time' and 'maxVelocity' and 'maxAccelerations' in all consecutive points; 'maxVelocities' and 'maxAccelerations' must be positive nonzero values that limit velocities and accelerations;
# joint: joint number for which the trajectory shall be evaluated
#**output: for current time t it returns [s, v, a] with path s, velocity v and acceleration a for current acceleration profile; outside of profile, it returns [0,0,0] !
#**notes: DEPRECATED, DO NOT USE - moved to robotics.motion
#**example:
# q0 = [0,0,0,0,0,0] #initial configuration
# q1 = [8,5,2,0,2,1] #other configuration
# PTP =[]
# PTP+=[{'q':q0,
# 'time':0}]
# PTP+=[{'q':q1,
# 'time':0.5}]
# PTP+=[{'q':q1,
# 'time':1e6}] #forever
# RT={'PTP':PTP}
# [u,v,a] = MotionInterpolator(t=0.5, robotTrajectory=RT, joint=1)
def MotionInterpolator(t, robotTrajectory, joint):
n = len(robotTrajectory['PTP'])
if n < 2:
print("ERROR in MotionInterpolator: trajectory must have at least 2 points!")
i = 0
while (i < n) and (t >= robotTrajectory['PTP'][i]['time']):
i += 1
if (i==0) or (i==n):
return [0,0,0] #outside of trajectory
#i must be > 0 and < n now!
q0 = robotTrajectory['PTP'][i-1] #must always exist
q1 = robotTrajectory['PTP'][i] #must always exist
return ConstantAccelerationProfile(t, q0['time'], q0['q'][joint],
q1['time'] - q0['time'],
q1['q'][joint] - q0['q'][joint])
#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#settings:
useGraphics= True
useContact = True
useFriction = True
# kProp = 10
dryFriction = 2*0.5
contactStiffness = 1e5
contactDamping = 2e-3*contactStiffness
wheelMass = 1
wheelInertia = 0.01
rotationDampingWheels = 0.01 #proportional to rotation speed
torque = 1
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#create circles
#complicated shape:
nANCFnodes = 1*50
stepSize = 5e-4
tEnd = 10
R=0.45
preStretch=-0.002*0 #not needed here, system is open!
circleList = [
[[ 0,-3 ],R,'L'],
[[ 0,0 ],R,'L'],
[[0.5,-0.8],R,'R'],
[[ 1,0 ],R,'L'],
[[ 1,-3 ],R,'L'],
]
#create motion profile:
point0={'q':[0], #initial configuration
'time':0}
point1={'q':[2.5/R],
'time':2}
point2={'q':[-2.5/R],
'time':4}
point3={'q':[0],
'time':5}
pointLast={'q':[0], #add this a second time to stay this forever
'time':1e6} #forever
RT={'PTP':[point0,point1,point2,point3,pointLast]}
#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#create geometry:
reevingDict = CreateReevingCurve(circleList, drawingLinesPerCircle = 64,
removeLastLine=False, #allows closed curve
numberOfANCFnodes=nANCFnodes, graphicsNodeSize= 0.01)
del circleList[-1] #remove circles not needed for contact/visualization
del circleList[0] #remove circles not needed for contact/visualization
gList=[]
if False: #visualize reeving curve, without simulation
gList = reevingDict['graphicsDataLines'] + reevingDict['graphicsDataCircles']
oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
visualization=VObjectGround(graphicsData= gList)))
#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#create ANCF elements:
gVec = np.array([0,-9.81,0]) # gravity
E=1e9 # Young's modulus of ANCF element in N/m^2
rhoBeam=1000 # density of ANCF element in kg/m^3
b=0.002 # width of rectangular ANCF element in m
h=0.002 # height of rectangular ANCF element in m
A=b*h # cross sectional area of ANCF element in m^2
I=b*h**3/12 # second moment of area of ANCF element in m^4
dEI = 1e-3*E*I #bending proportional damping
dEA = 1e-2*E*A #axial strain proportional damping
dimZ = b #z.dimension
cableTemplate = Cable2D(#physicsLength = L / nElements, #set in GenerateStraightLineANCFCable2D(...)
physicsMassPerLength = rhoBeam*A,
physicsBendingStiffness = E*I,
physicsAxialStiffness = E*A,
physicsBendingDamping = dEI,
physicsAxialDamping = dEA,
physicsReferenceAxialStrain = preStretch, #prestretch
visualization=VCable2D(drawHeight=h),
)
ancf = PointsAndSlopes2ANCFCable2D(mbs, reevingDict['ancfPointsSlopes'], reevingDict['elementLengths'],
cableTemplate, massProportionalLoad=gVec,
#fixedConstraintsNode0=[1,1,1,1], fixedConstraintsNode1=[1,1,1,1],
firstNodeIsLastNode=False, graphicsSizeConstraints=0.01)
#+++++++++++++++++++++++++++++++++++++++
#add weights
node0 = ancf[0][0]
nodeL = ancf[0][-1]
massLoad=2
bMass0 = mbs.AddObject(ObjectMassPoint2D(physicsMass=massLoad,
nodeNumber=node0,
visualization=VMassPoint2D(graphicsData=[GraphicsDataSphere(radius=0.1, nTiles=32)])))
bMassL = mbs.AddObject(ObjectMassPoint2D(physicsMass=massLoad,
nodeNumber=nodeL,
visualization=VMassPoint2D(graphicsData=[GraphicsDataSphere(radius=0.1, nTiles=32)])))
mBody0=mbs.AddMarker(MarkerBodyPosition(bodyNumber=bMass0))
mbs.AddLoad(Force(markerNumber=mBody0, loadVector=massLoad*gVec))
mBodyL=mbs.AddMarker(MarkerBodyPosition(bodyNumber=bMassL))
mbs.AddLoad(Force(markerNumber=mBodyL, loadVector=massLoad*gVec))
#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#add contact:
if useContact:
gContact = mbs.AddGeneralContact()
gContact.verboseMode = 1
gContact.frictionProportionalZone = 0.5
gContact.ancfCableUseExactMethod = False
gContact.ancfCableNumberOfContactSegments = 4
gContact
ssx = 32#32 #search tree size
ssy = 32#32 #search tree size
ssz = 1 #search tree size
gContact.SetSearchTreeCellSize(numberOfCells=[ssx,ssy,ssz])
#gContact.SetSearchTreeBox(pMin=np.array([-1,-1,-1]), pMax=np.array([4,1,1])) #automatically computed!
halfHeight = 0.5*h*0
dimZ= 0.01 #for drawing
# wheels = [{'center':wheelCenter0, 'radius':rWheel0-halfHeight, 'mass':mWheel},
# {'center':wheelCenter1, 'radius':rWheel1-halfHeight, 'mass':mWheel},
# {'center':rollCenter1, 'radius':rRoll-halfHeight, 'mass':mRoll}, #small mass for roll, not to influence beam
# ]
sWheelRot = [] #sensors for angular velocity
nGround = mbs.AddNode(NodePointGround())
mCoordinateGround = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nGround, coordinate=0))
for i, wheel in enumerate(circleList):
p = [wheel[0][0], wheel[0][1], 0] #position of wheel center
r = wheel[1]
rot0 = 0 #initial rotation
pRef = [p[0], p[1], rot0]
gList = [GraphicsDataCylinder(pAxis=[0,0,-dimZ],vAxis=[0,0,-dimZ], radius=r,
color= color4lightgrey, nTiles=64),
GraphicsDataArrow(pAxis=[0,0,0], vAxis=[0.9*r,0,0], radius=0.01*r, color=color4orange)]
omega0 = 0 #initial angular velocity
v0 = np.array([0,0,omega0])
nMass = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=pRef, initialVelocities=v0,
visualization=VNodeRigidBody2D(drawSize=dimZ*2)))
oMass = mbs.AddObject(ObjectRigidBody2D(physicsMass=wheelMass, physicsInertia=wheelInertia,
nodeNumber=nMass, visualization=
VObjectRigidBody2D(graphicsData=gList)))
mNode = mbs.AddMarker(MarkerNodeRigid(nodeNumber=nMass))
mGroundWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=p))
mbs.AddObject(RevoluteJoint2D(markerNumbers=[mGroundWheel, mNode]))
sWheelRot += [mbs.AddSensor(SensorNode(nodeNumber=nMass,
fileName='solution/wheel'+str(i)+'angVel.txt',
outputVariableType=exu.OutputVariableType.AngularVelocity))]
#add drive with prescribed velocity:
def UFvelocityDrive(mbs, t, itemNumber, lOffset): #time derivative of UFoffset
v = 10*t
vMax = 5
return max(v, vMax)
def UFmotionDrive(mbs, t, itemNumber, lOffset):
[u,v,a] = MotionInterpolator(t, robotTrajectory=RT, joint=0)
return u
velocityControl = False
if i == 1:
mCoordinateWheel = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nMass, coordinate=2))
if velocityControl:
mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround, mCoordinateWheel],
velocityLevel=True, offsetUserFunction_t=UFvelocityDrive))
else: #position control
mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround, mCoordinateWheel],
offsetUserFunction=UFmotionDrive))
if i > 0: #friction on rolls:
mCoordinateWheel = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nMass, coordinate=2))
mbs.AddObject(CoordinateSpringDamper(markerNumbers=[mCoordinateGround, mCoordinateWheel],
damping=rotationDampingWheels,
visualization=VCoordinateSpringDamper(show=False)))
frictionMaterialIndex=0
gContact.AddSphereWithMarker(mNode, radius=r, contactStiffness=contactStiffness,
contactDamping=contactDamping, frictionMaterialIndex=frictionMaterialIndex)
for oIndex in ancf[1]:
gContact.AddANCFCable(objectIndex=oIndex, halfHeight=halfHeight, #halfHeight should be h/2, but then cylinders should be smaller
contactStiffness=contactStiffness, contactDamping=contactDamping,
frictionMaterialIndex=0)
frictionMatrix = np.zeros((2,2))
frictionMatrix[0,0]=int(useFriction)*dryFriction
frictionMatrix[0,1]=0 #no friction between some rolls and cable
frictionMatrix[1,0]=0 #no friction between some rolls and cable
gContact.SetFrictionPairings(frictionMatrix)
mbs.Assemble()
simulationSettings = exu.SimulationSettings() #takes currently set values or default values
simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
simulationSettings.solutionSettings.coordinatesSolutionFileName = 'solution/coordinatesSolution.txt'
simulationSettings.solutionSettings.writeSolutionToFile = True
simulationSettings.solutionSettings.solutionWritePeriod = 0.005
simulationSettings.solutionSettings.sensorsWritePeriod = 0.001
#simulationSettings.displayComputationTime = True
simulationSettings.parallel.numberOfThreads = 4 #use 4 to speed up for > 100 ANCF elements
simulationSettings.displayStatistics = True
simulationSettings.timeIntegration.endTime = tEnd
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
simulationSettings.timeIntegration.stepInformation= 3+4+32#+128+256
#simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-4
simulationSettings.timeIntegration.newton.relativeTolerance = 1e-6
#simulationSettings.timeIntegration.discontinuous.iterationTolerance = 10
simulationSettings.timeIntegration.newton.modifiedNewtonJacUpdatePerStep=True
simulationSettings.timeIntegration.newton.maxIterations=16
simulationSettings.timeIntegration.verboseMode = 1
# simulationSettings.timeIntegration.adaptiveStepRecoveryIterations = 9
simulationSettings.timeIntegration.adaptiveStepRecoverySteps = 40
simulationSettings.timeIntegration.newton.useModifiedNewton = True
simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
simulationSettings.displayStatistics = True
SC.visualizationSettings.general.circleTiling = 24
SC.visualizationSettings.loads.show=False
SC.visualizationSettings.nodes.defaultSize = 0.01
SC.visualizationSettings.openGL.multiSampling = 4
SC.visualizationSettings.openGL.lineWidth = 2
# SC.visualizationSettings.general.useGradientBackground = True
simulationSettings.solutionSettings.solutionInformation = 'elevator'
# SC.visualizationSettings.general.textSize = 14
SC.visualizationSettings.window.renderWindowSize = [1024,2000]
if False:
SC.visualizationSettings.contour.outputVariableComponent=0
SC.visualizationSettings.contour.outputVariable=exu.OutputVariableType.ForceLocal
#visualize contact:
if False:
SC.visualizationSettings.contact.showSearchTree =True
SC.visualizationSettings.contact.showSearchTreeCells =True
SC.visualizationSettings.contact.showBoundingBoxes = True
if useGraphics:
exu.StartRenderer()
mbs.WaitForUserToContinue()
if True:
doDynamic = True
if doDynamic :
exu.SolveDynamic(mbs, simulationSettings) #183 Newton iterations, 0.114 seconds
else:
exu.SolveStatic(mbs, simulationSettings) #183 Newton iterations, 0.114 seconds
if useGraphics and True:
SC.visualizationSettings.general.autoFitScene = False
SC.visualizationSettings.general.graphicsUpdateInterval=0.02
from exudyn.interactive import SolutionViewer
sol = LoadSolutionFile('solution/coordinatesSolution.txt', safeMode=True)#, maxRows=100)
SolutionViewer(mbs, sol)
if useGraphics:
SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!
# if True:
# from exudyn.plot import PlotSensor
# PlotSensor(mbs, sensorNumbers=[sAngVel[0],sAngVel[1]], components=2, closeAll=True)
# PlotSensor(mbs, sensorNumbers=sMeasureRoll, components=1)