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