kinematicTreePendulum.py

You can view and download this file on Github: kinematicTreePendulum.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
 20from math import pi, sin, cos#, sqrt
 21from copy import copy, deepcopy
 22from exudyn.robotics import *
 23
 24SC = exu.SystemContainer()
 25mbs = SC.AddSystem()
 26
 27useGraphics = True
 28
 29gGround =  graphics.CheckerBoard(point= [0,0,-2], size = 12)
 30objectGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
 31                                          visualization=VObjectGround(graphicsData=[gGround])))
 32
 33L = 0.5 #length
 34w = 0.1 #width of links
 35
 36gravity3D = [0,-9.81*1,0]
 37graphicsBaseList = [graphics.Brick(size=[L*4, 0.8*w, 0.8*w], color=graphics.color.grey)] #rail
 38
 39newRobot = Robot(gravity=gravity3D,
 40              base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
 41              tool = RobotTool(HT=HTtranslate([0,0.5*L,0]), visualization=VRobotTool(graphicsData=[
 42                  graphics.Brick(size=[w, L, w], color=graphics.color.orange)])),
 43              referenceConfiguration = []) #referenceConfiguration created with 0s automatically
 44
 45linksList = []
 46
 47nChainLinks = 1 #5
 48for i in range(nChainLinks):
 49    Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
 50    Jlink = Jlink.Translated([0,0.5*L,0])
 51    preHT = HT0()
 52    if i > 0:
 53        preHT = HTtranslateY(L)
 54
 55    link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
 56                     jointType='Rz', preHT=preHT,
 57                     #PDcontrol=(pControl*0, dControl*0),
 58                     visualization=VRobotLink(linkColor=graphics.color.blue))
 59    newRobot.AddLink(link)
 60    linksList += [copy(link)]
 61
 62#newRobot.referenceConfiguration[0] = 0.5*0
 63# for i in range(nChainLinks):
 64#     newRobot.referenceConfiguration[i+1] = (2*pi/360) * 5
 65newRobot.referenceConfiguration[0] = -(2*pi/360) * 90 #-0.5*pi
 66# newRobot.referenceConfiguration[2] = (2*pi/360) * 12 #-0.5*pi
 67
 68
 69dKT = newRobot.CreateKinematicTree(mbs)
 70oKT = dKT['objectKinematicTree']
 71
 72sCoords=mbs.AddSensor(SensorBody(bodyNumber=oKT, storeInternal=True,
 73                    outputVariableType=exu.OutputVariableType.Coordinates))
 74
 75mbs.Assemble()
 76
 77simulationSettings = exu.SimulationSettings()
 78
 79tEnd = 2000
 80h = 1e-2#*0.01
 81#tEnd = h
 82
 83simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
 84simulationSettings.timeIntegration.endTime = tEnd
 85simulationSettings.solutionSettings.writeSolutionToFile=False
 86simulationSettings.solutionSettings.sensorsWritePeriod = 0.05
 87simulationSettings.timeIntegration.verboseMode = 1
 88
 89simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.95 #SHOULD work with 0.9 as well
 90
 91SC.visualizationSettings.general.autoFitScene=False
 92SC.visualizationSettings.window.renderWindowSize = [1600,1200]
 93SC.visualizationSettings.general.drawCoordinateSystem=True
 94SC.visualizationSettings.general.drawWorldBasis=True
 95SC.visualizationSettings.openGL.multiSampling=4
 96SC.visualizationSettings.nodes.showBasis = True
 97SC.visualizationSettings.nodes.basisSize = 0.5
 98
 99if useGraphics:
100
101    exu.StartRenderer()
102    if 'renderState' in exu.sys: SC.SetRenderState(exu.sys['renderState']) #load last model view
103
104    mbs.WaitForUserToContinue() #press space to continue
105
106
107
108
109mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.ExplicitMidpoint)
110mbs.PlotSensor(sensorNumbers=sCoords, components=0, labels='Explicit Midpoint', colorCodeOffset=2, closeAll=True)
111
112mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.RK33)
113mbs.PlotSensor(sensorNumbers=sCoords, components=0, labels='Heun', colorCodeOffset=1, newFigure=False)
114
115mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.RK44)
116mbs.PlotSensor(sensorNumbers=sCoords, components=0, labels='Runge Kutta 44', newFigure=False)
117
118
119#mbs.SolveDynamic(simulationSettings)
120
121simulationSettings.timeIntegration.numberOfSteps = int(7/h)
122simulationSettings.timeIntegration.endTime = 7
123mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.ExplicitEuler)
124mbs.PlotSensor(sensorNumbers=sCoords, components=0, yLabel='pendulum angle', labels=['Explicit Euler'], colorCodeOffset=3, newFigure=False)
125
126if useGraphics:
127    #SC.WaitForRenderEngineStopFlag()
128    exu.StopRenderer() #safely close rendering window!