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