kinematicTreeConstraintTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN python utility library
  3#
  4# Details:  test of MarkerKinematicTreeRigid in combination with loads and joint
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2022-05-29
  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
 16from exudyn.FEM import *
 17
 18import numpy as np
 19
 20useGraphics = True
 21#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 22#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 23try: #only if called from test suite
 24    from modelUnitTests import exudynTestGlobals #for globally storing test results
 25    useGraphics = exudynTestGlobals.useGraphics
 26except:
 27    class ExudynTestGlobals:
 28        pass
 29    exudynTestGlobals = ExudynTestGlobals()
 30#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 31
 32from math import pi, sin, cos#, sqrt
 33from copy import copy, deepcopy
 34from exudyn.rigidBodyUtilities import Skew, Skew2Vec
 35from exudyn.robotics import *
 36
 37SC = exu.SystemContainer()
 38mbs = SC.AddSystem()
 39
 40# useGraphics = False
 41
 42useMBS = True
 43useKinematicTree = True
 44addForce = True #add gravity as body / link forces
 45addConstraint = True #add constraint at tip of chain
 46
 47
 48gGround =  graphics.CheckerBoard(point= [0,0,-2], size = 12)
 49objectGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
 50                                          visualization=VObjectGround(graphicsData=[gGround])))
 51baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
 52
 53L = 0.5 #length
 54w = 0.1 #width of links
 55pControl = 20000 #we keep the motion of the prismatic joint fixed
 56dControl = pControl*0.02
 57
 58gravity3D = [0,-9.81*0,0]
 59graphicsBaseList = [graphics.Brick(size=[L*4, 0.8*w, 0.8*w], color=graphics.color.grey)] #rail
 60
 61newRobot = Robot(gravity=gravity3D,
 62              base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
 63              tool = RobotTool(HT=HTtranslate([0,0.5*L,0]), visualization=VRobotTool(graphicsData=[
 64                  graphics.Brick(size=[w, L, w], color=graphics.color.orange)])),
 65              referenceConfiguration = []) #referenceConfiguration created with 0s automatically
 66
 67#cart:
 68Jlink = InertiaCuboid(density=5000, sideLengths=[L,w,w]) #w.r.t. reference center of mass
 69link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
 70                 jointType='Px', preHT=HT0(),
 71                 PDcontrol=(pControl, dControl),
 72                 visualization=VRobotLink(linkColor=graphics.color.lawngreen))
 73newRobot.AddLink(link)
 74linksList = [copy(link)]
 75
 76nChainLinks = 4 #5
 77for i in range(nChainLinks):
 78    Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
 79    Jlink = Jlink.Translated([0,0.5*L,0])
 80    preHT = HT0()
 81    if i > 0:
 82        preHT = HTtranslateY(L)
 83
 84    link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
 85                     jointType='Rz', preHT=preHT,
 86                     PDcontrol=(pControl*0, dControl*0),
 87                     visualization=VRobotLink(linkColor=graphics.color.blue))
 88    newRobot.AddLink(link)
 89    linksList += [copy(link)]
 90
 91newRobot.referenceConfiguration[0] = 0.5*0
 92# for i in range(nChainLinks):
 93#     newRobot.referenceConfiguration[i+1] = (2*pi/360) * 5
 94newRobot.referenceConfiguration[1] = -(2*pi/360) * 90 #-0.5*pi
 95# newRobot.referenceConfiguration[2] = (2*pi/360) * 12 #-0.5*pi
 96
 97# locPos = [0.1,0.2,0.3]
 98locPos = [0,0,0]
 99nLinks = newRobot.NumberOfLinks()
100
101sMBS = []
102if useMBS:
103    #newRobot.gravity=[0,-9.81,0]
104    robDict = newRobot.CreateRedundantCoordinateMBS(mbs=mbs, baseMarker=baseMarker, createJointTorqueLoads=False)
105    bodies = robDict['bodyList']
106
107    sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
108                                    outputVariableType=exu.OutputVariableType.Position))]
109
110    if addForce:
111        for i in range(len(bodies)):
112            mBody = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bodies[i], localPosition=linksList[i].COM))
113            mbs.AddLoad(Force(markerNumber=mBody, loadVector=[0,-9.81*linksList[i].mass, 0]))
114
115    if addConstraint:
116        mTip = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bodies[-1], localPosition=[0,L,0]))
117        mTipGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[L*nChainLinks,0,0]))
118        mbs.AddObject(SphericalJoint(markerNumbers=[mTip, mTipGround], constrainedAxes=[0,1,0]))
119
120sKT = []
121if useKinematicTree:
122    #newRobot.gravity=[0,-9.81,0]
123    dKT = newRobot.CreateKinematicTree(mbs)
124    oKT = dKT['objectKinematicTree']
125
126    sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
127                                            outputVariableType=exu.OutputVariableType.Position))]
128
129    if addForce:
130        for i in range(nLinks):
131            mLink = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=i, localPosition=linksList[i].COM))
132            mbs.AddLoad(Force(markerNumber=mLink, loadVector=[0,-9.81*linksList[i].mass, 0]))
133
134    if addConstraint:
135        mTip = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=nLinks-1, localPosition=[0,L,0]))
136        mTipGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber = objectGround, localPosition=[L*nChainLinks,0,0]))
137        mbs.AddObject(SphericalJoint(markerNumbers=[mTip, mTipGround], constrainedAxes=[0,1,0]))
138
139#exu.Print(mbs)
140mbs.Assemble()
141
142simulationSettings = exu.SimulationSettings()
143
144tEnd = 0.5
145h = 4*1e-3
146#tEnd = h
147
148simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
149simulationSettings.timeIntegration.endTime = tEnd
150# simulationSettings.timeIntegration.numberOfSteps = 1#int(tEnd/h)
151# simulationSettings.timeIntegration.endTime = h*1#tEnd
152simulationSettings.solutionSettings.solutionWritePeriod = 0.01*100
153simulationSettings.solutionSettings.sensorsWritePeriod = 0.001*20
154simulationSettings.timeIntegration.verboseMode = 1
155#simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
156simulationSettings.timeIntegration.newton.useModifiedNewton=True
157
158# simulationSettings.displayComputationTime = True
159# simulationSettings.linearSolverType=exu.LinearSolverType.EigenSparse
160
161simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.95 #SHOULD work with 0.9 as well
162
163SC.visualizationSettings.general.autoFitScene=False
164SC.visualizationSettings.window.renderWindowSize = [1600,1200]
165SC.visualizationSettings.general.drawCoordinateSystem=True
166SC.visualizationSettings.general.drawWorldBasis=True
167SC.visualizationSettings.openGL.multiSampling=4
168SC.visualizationSettings.nodes.showBasis = True
169SC.visualizationSettings.nodes.basisSize = 0.5
170if useGraphics:
171
172    exu.StartRenderer()
173    if 'renderState' in exu.sys: SC.SetRenderState(exu.sys['renderState']) #load last model view
174
175    mbs.WaitForUserToContinue() #press space to continue
176
177# mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.ExplicitMidpoint)
178mbs.SolveDynamic(simulationSettings)
179
180if not useGraphics or True:
181    #check results for test suite:
182    u = 0.
183    for i in range(len(sMBS)):
184        v = mbs.GetSensorValues(sMBS[i])
185        exu.Print('sensor MBS '+str(i)+'=',v)
186        u += np.linalg.norm(v)
187        v = mbs.GetSensorValues(sKT[i])
188        exu.Print('sensor KT '+str(i)+' =',v)
189        u += np.linalg.norm(v)
190
191exu.Print("solution of kinematicTreeConstraintTest=", u)
192exudynTestGlobals.testResult = u #1.8135975385993548
193
194
195if False and useGraphics: #use this to reload the solution and use SolutionViewer
196    #sol = LoadSolutionFile('coordinatesSolution.txt')
197
198    mbs.SolutionViewer() #can also be entered in IPython ...
199
200if useGraphics:
201    SC.WaitForRenderEngineStopFlag()
202    exu.StopRenderer() #safely close rendering window!