You can view and download this file on Github: kinematicTreeConstraintTest.py
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN python utility library
#
# Details: test of MarkerKinematicTreeRigid in combination with loads and joint
#
# Author: Johannes Gerstmayr
# Date: 2022-05-29
#
# 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.utilities import *
from exudyn.FEM import *
import numpy as np
useGraphics = True
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
try: #only if called from test suite
from modelUnitTests import exudynTestGlobals #for globally storing test results
useGraphics = exudynTestGlobals.useGraphics
except:
class ExudynTestGlobals:
pass
exudynTestGlobals = ExudynTestGlobals()
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
from math import pi, sin, cos#, sqrt
from copy import copy, deepcopy
from exudyn.rigidBodyUtilities import Skew, Skew2Vec
from exudyn.robotics import *
SC = exu.SystemContainer()
mbs = SC.AddSystem()
# useGraphics = False
useMBS = True
useKinematicTree = True
addForce = True #add gravity as body / link forces
addConstraint = True #add constraint at tip of chain
gGround = GraphicsDataCheckerBoard(point= [0,0,-2], size = 12)
objectGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
visualization=VObjectGround(graphicsData=[gGround])))
baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
L = 0.5 #length
w = 0.1 #width of links
pControl = 20000 #we keep the motion of the prismatic joint fixed
dControl = pControl*0.02
gravity3D = [0,-9.81*0,0]
graphicsBaseList = [GraphicsDataOrthoCubePoint(size=[L*4, 0.8*w, 0.8*w], color=color4grey)] #rail
newRobot = Robot(gravity=gravity3D,
base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
tool = RobotTool(HT=HTtranslate([0,0.5*L,0]), visualization=VRobotTool(graphicsData=[
GraphicsDataOrthoCubePoint(size=[w, L, w], color=color4orange)])),
referenceConfiguration = []) #referenceConfiguration created with 0s automatically
#cart:
Jlink = InertiaCuboid(density=5000, sideLengths=[L,w,w]) #w.r.t. reference center of mass
link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
jointType='Px', preHT=HT0(),
PDcontrol=(pControl, dControl),
visualization=VRobotLink(linkColor=color4lawngreen))
newRobot.AddLink(link)
linksList = [copy(link)]
nChainLinks = 4 #5
for i in range(nChainLinks):
Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
Jlink = Jlink.Translated([0,0.5*L,0])
preHT = HT0()
if i > 0:
preHT = HTtranslateY(L)
link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
jointType='Rz', preHT=preHT,
PDcontrol=(pControl*0, dControl*0),
visualization=VRobotLink(linkColor=color4blue))
newRobot.AddLink(link)
linksList += [copy(link)]
newRobot.referenceConfiguration[0] = 0.5*0
# for i in range(nChainLinks):
# newRobot.referenceConfiguration[i+1] = (2*pi/360) * 5
newRobot.referenceConfiguration[1] = -(2*pi/360) * 90 #-0.5*pi
# newRobot.referenceConfiguration[2] = (2*pi/360) * 12 #-0.5*pi
# locPos = [0.1,0.2,0.3]
locPos = [0,0,0]
nLinks = newRobot.NumberOfLinks()
sMBS = []
if useMBS:
#newRobot.gravity=[0,-9.81,0]
robDict = newRobot.CreateRedundantCoordinateMBS(mbs=mbs, baseMarker=baseMarker, createJointTorqueLoads=False)
bodies = robDict['bodyList']
sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
outputVariableType=exu.OutputVariableType.Position))]
if addForce:
for i in range(len(bodies)):
mBody = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bodies[i], localPosition=linksList[i].COM))
mbs.AddLoad(Force(markerNumber=mBody, loadVector=[0,-9.81*linksList[i].mass, 0]))
if addConstraint:
mTip = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bodies[-1], localPosition=[0,L,0]))
mTipGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[L*nChainLinks,0,0]))
mbs.AddObject(SphericalJoint(markerNumbers=[mTip, mTipGround], constrainedAxes=[0,1,0]))
sKT = []
if useKinematicTree:
#newRobot.gravity=[0,-9.81,0]
dKT = newRobot.CreateKinematicTree(mbs)
oKT = dKT['objectKinematicTree']
sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
outputVariableType=exu.OutputVariableType.Position))]
if addForce:
for i in range(nLinks):
mLink = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=i, localPosition=linksList[i].COM))
mbs.AddLoad(Force(markerNumber=mLink, loadVector=[0,-9.81*linksList[i].mass, 0]))
if addConstraint:
mTip = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=nLinks-1, localPosition=[0,L,0]))
mTipGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber = objectGround, localPosition=[L*nChainLinks,0,0]))
mbs.AddObject(SphericalJoint(markerNumbers=[mTip, mTipGround], constrainedAxes=[0,1,0]))
#exu.Print(mbs)
mbs.Assemble()
simulationSettings = exu.SimulationSettings()
tEnd = 0.5
h = 4*1e-3
#tEnd = h
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
simulationSettings.timeIntegration.endTime = tEnd
# simulationSettings.timeIntegration.numberOfSteps = 1#int(tEnd/h)
# simulationSettings.timeIntegration.endTime = h*1#tEnd
simulationSettings.solutionSettings.solutionWritePeriod = 0.01*100
simulationSettings.solutionSettings.sensorsWritePeriod = 0.001*20
simulationSettings.timeIntegration.verboseMode = 1
#simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
simulationSettings.timeIntegration.newton.useModifiedNewton=True
# simulationSettings.displayComputationTime = True
# simulationSettings.linearSolverType=exu.LinearSolverType.EigenSparse
simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.95 #SHOULD work with 0.9 as well
SC.visualizationSettings.general.autoFitScene=False
SC.visualizationSettings.window.renderWindowSize = [1600,1200]
SC.visualizationSettings.general.drawCoordinateSystem=True
SC.visualizationSettings.general.drawWorldBasis=True
SC.visualizationSettings.openGL.multiSampling=4
SC.visualizationSettings.nodes.showBasis = True
SC.visualizationSettings.nodes.basisSize = 0.5
if useGraphics:
exu.StartRenderer()
if 'renderState' in exu.sys: SC.SetRenderState(exu.sys['renderState']) #load last model view
mbs.WaitForUserToContinue() #press space to continue
# mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.ExplicitMidpoint)
mbs.SolveDynamic(simulationSettings)
if not useGraphics or True:
#check results for test suite:
u = 0.
for i in range(len(sMBS)):
v = mbs.GetSensorValues(sMBS[i])
exu.Print('sensor MBS '+str(i)+'=',v)
u += np.linalg.norm(v)
v = mbs.GetSensorValues(sKT[i])
exu.Print('sensor KT '+str(i)+' =',v)
u += np.linalg.norm(v)
exu.Print("solution of kinematicTreeConstraintTest=", u)
exudynTestGlobals.testResult = u #1.8135975385993548
if False and useGraphics: #use this to reload the solution and use SolutionViewer
#sol = LoadSolutionFile('coordinatesSolution.txt')
mbs.SolutionViewer() #can also be entered in IPython ...
if useGraphics:
SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!