kinematicTreeAndMBS.py
You can view and download this file on Github: kinematicTreeAndMBS.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN python utility library
3#
4# Details: Test for kinematicTree; this test uses different versions of Python and C++ implementations for kinematic tree (implementation may not be optimal or have some unnecessary overheads);
5# The Python tests are not efficient, so use only C++ ObjectKinematicTree for performance reasons!
6#
7# Author: Johannes Gerstmayr
8# Date: 2021-08-20
9#
10# 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.
11#
12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
14#constants and fixed structures:
15import numpy as np
16from math import pi, sin, cos#, sqrt
17from copy import copy, deepcopy
18
19import exudyn as exu
20from exudyn.utilities import *
21from exudyn.rigidBodyUtilities import Skew, Skew2Vec
22from exudyn.robotics import *
23
24from exudyn.kinematicTree import *
25
26SC = exu.SystemContainer()
27mbs = SC.AddSystem()
28
29
30#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31gGround = GraphicsDataOrthoCubePoint(centerPoint=[-0.5,0,0], size=[1,0.4,0.5], color=color4lightgrey)
32objectGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
33 visualization=VObjectGround(graphicsData=[gGround])))
34
35L = 2 #length of links
36w = 0.1 #width of links
37J = InertiaCuboid(density=1000, sideLengths=[L,w,w]) #w.r.t. reference center of mass
38#J.com = np.array([0.5*L,0,0])
39J = J.Translated([0.5*L,0,0])
40
41com = J.com
42Jcom = J.Translated(-com)
43
44gravity3D = [0,-10,0]
45
46n=5 #number of coordinates
47Amat = [np.zeros((3,3))]*n
48vVec = [np.zeros(3)]*n
49
50listCOM = [np.zeros(3)]*n
51listInertiaCOM = [np.zeros((3,3))]*n
52listInertia = [np.zeros((3,3))]*n
53listMass = [0]*n
54for i in range(n):
55 A=np.eye(3)
56 if i%2 != 0:
57 A=RotXYZ2RotationMatrix([0*0.5*pi,0.25*pi,0])
58 if i%3 >= 1:
59 A=RotXYZ2RotationMatrix([0.5*pi,0.25*pi,0])
60
61 v = np.array([L,0,0])
62 if i==0:
63 v = np.array([0,0,0])
64 Amat[i] = A
65 vVec[i] = v
66
67 listMass[i] = J.mass
68 listCOM[i] = J.com
69 listInertia[i] = J.inertiaTensor
70 listInertiaCOM[i] = Jcom.inertiaTensor
71
72useKT = True
73useMBS = False #for this option, choose dynamic solver!
74useKT2 = False
75useKTcpp = True
76sJointsList = []
77sCases = []
78#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
79baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
80graphicsBaseList = [gGround]
81graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0.5,0,0], 0.0025, color4red)]
82graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0.5,0], 0.0025, color4green)]
83graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0,0.5], 0.0025, color4blue)]
84newRobot = Robot(gravity=gravity3D,
85 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
86 tool = RobotTool(HT=HTtranslate([0,0,0]), visualization=VRobotTool(graphicsData=[])),
87 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
88
89for i in range(n):
90 newRobot.AddLink(RobotLink(mass=listMass[i],
91 COM=listCOM[i],
92 inertia=listInertiaCOM[i],
93 preHT = HomogeneousTransformation(Amat[i], vVec[i]),
94 ))
95
96if useMBS:
97 robDict = newRobot.CreateRedundantCoordinateMBS(mbs=mbs, baseMarker=baseMarker, createJointTorqueLoads=False)
98 # invertJoints=True)
99 joints = robDict['jointList']
100 #user function for sensor, convert position into angle:
101 def UFsensor(mbs, t, sensorNumbers, factors, configuration):
102 val = np.zeros(n)
103 for i in range(n):
104 val[i] = mbs.GetObjectOutput(joints[i], exu.OutputVariableType.Rotation)[2] #z-rotation
105 return val #return joint angles
106
107 sJointsList += [mbs.AddSensor(SensorUserFunction(sensorNumbers=[],
108 fileName='solution/serialRobMBS.txt',
109 sensorUserFunction=UFsensor))]
110 sCases += ['RC MBS']
111
112#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
113#generate kinematic chain:
114jointTypes = []
115transformations = []
116inertias = []
117for i in range(n):
118 jointTypes += ['Rz']
119 X=RotationTranslation2T66Inverse(A=Amat[i], v=vVec[i]) #A is the transformation from (i) to (i-1), while the Featherstone formalism requires the transformation from (i-1) to i
120 #print(X.round(3))
121 transformations += [X] #defines transformation to joint in parent link
122 inertias += [Inertia2T66(J)]
123
124if True: #manual mode
125 KT=KinematicTree66(listOfJointTypes=jointTypes, listOfTransformations=transformations,
126 listOfInertias=inertias, gravity=gravity3D)
127else:
128 KT = newRobot.GetKinematicTree66()
129
130# acc=KT.ForwardDynamicsCRB([0]*n,[0]*n)
131# print("acc=",acc)
132#[M,f]=KT.ComputeMassMatrixAndForceTerms([1]*n,[2]*n)
133# print("M=",M.round(3), ",\n f=", f)
134
135
136
137
138if useKT:
139 def UFgenericODE2(mbs, t, itemIndex, q, q_t):
140 #f = np.array([sin(t*2*pi)*0.01]*n)
141 [M,f]=KT.ComputeMassMatrixAndForceTerms(q, q_t)
142 #exu.Print("t =", t, ", f =", f)
143 return -f
144
145 M=np.eye(n)
146 def UFmassGenericODE2(mbs, t, itemIndex, q, q_t):
147 [M,f]=KT.ComputeMassMatrixAndForceTerms(q,q_t)
148 return M
149
150 def UFgraphics(mbs, itemNumber):
151 #t = mbs.systemData.GetTime(exu.ConfigurationType.Visualization) #get time if needed
152 nn = mbs.GetObjectParameter(itemNumber,'nodeNumbers')[0] #get first node
153 q = mbs.GetNodeOutput(nodeNumber=nn, variableType = exu.OutputVariableType.Coordinates,
154 configuration = exu.ConfigurationType.Visualization)
155 if isinstance(q,float): #q is scalar if dimension is 1
156 q=[q]
157
158 graphicsList = []
159 T66 = np.eye(6) #initial transformation
160
161 #only valid for chains: !!!!
162 pPrev = [0,0,0]
163 for i in range(n):
164 # T66prev = T66
165 [XJ, MS] = JointTransformMotionSubspace66(KT.jointTypes[i], q[i])
166 T66 = XJ @ KT.XL(i) @ T66
167
168 [A, p] = T66toRotationTranslation(T66)
169 p = -A.T@p
170 A = A.T
171
172 # alternative:
173 # H = T66toHT(T66Inverse(T66))
174 # p = HT2translation(H)
175 # A = HT2rotationMatrix(H)
176
177 vAxis = A@np.array([0,0,0.25*L])
178 graphicsList += [GraphicsDataCylinder(pAxis=p-0.5*vAxis, vAxis=vAxis, radius=0.05*L, color=color4red)]
179 cube= GraphicsDataOrthoCubePoint(centerPoint=[0.5*L,0,0], size=[L,0.08*L,0.15*L], color=color4brown)
180 cube2 = MoveGraphicsData(cube, p, A)
181 graphicsList += [cube2]
182 #graphicsList += [{'type':'Line', 'data': list(p)+list(p0), 'color':color4blue}]
183 return graphicsList
184
185 nGeneric=mbs.AddNode(NodeGenericODE2(referenceCoordinates = [0]*n,
186 initialCoordinates = [0]*n,
187 initialCoordinates_t= [0]*n,
188 numberOfODE2Coordinates = n))
189
190 mbs.AddObject(ObjectGenericODE2(nodeNumbers = [nGeneric],
191 # massMatrix=M,
192 # stiffnessMatrix=K,
193 # dampingMatrix=D,
194 # forceVector=fv,
195 forceUserFunction=UFgenericODE2,
196 massMatrixUserFunction=UFmassGenericODE2,
197 visualization=VObjectGenericODE2(graphicsDataUserFunction=UFgraphics)))
198
199 sJointsList += [mbs.AddSensor(SensorNode(nodeNumber=nGeneric, fileName='solution/genericNode.txt',
200 outputVariableType=exu.OutputVariableType.Coordinates))]
201 sCases += [' KT T66']
202
203
204if useKT2:
205 KT2=KinematicTree33(listOfJointTypes=jointTypes,
206 listOfRotations=Amat,
207 listOfOffsets=vVec,
208 listOfInertia3D=listInertia,
209 listOfCOM=listCOM,
210 listOfMass=listMass,
211 gravity=gravity3D)
212 # acc=KT.ForwardDynamicsCRB([0]*n,[0]*n)
213 # print("acc=",acc)
214 [M2,f2]=KT2.ComputeMassMatrixAndForceTerms([1]*n,[2]*n)
215 # print("M2=",M2.round(3), ",\n f2=", f2)
216 def UFgenericODE2KT2(mbs, t, itemIndex, q, q_t):
217 #f = np.array([sin(t*2*pi)*0.01]*n)
218 [M,f]=KT2.ComputeMassMatrixAndForceTerms(q, q_t)
219 #exu.Print("t =", t, ", f =", f)
220 return -f
221
222 M=np.eye(n)
223 def UFmassGenericODE2KT2(mbs, t, itemIndex, q, q_t):
224 [M,f]=KT2.ComputeMassMatrixAndForceTerms(q,q_t)
225 return M
226
227 def UFgraphicsKT2(mbs, itemNumber):
228 #t = mbs.systemData.GetTime(exu.ConfigurationType.Visualization) #get time if needed
229 nn = mbs.GetObjectParameter(itemNumber,'nodeNumbers')[0] #get first node
230 q = mbs.GetNodeOutput(nodeNumber=nn, variableType = exu.OutputVariableType.Coordinates,
231 configuration = exu.ConfigurationType.Visualization)
232 if isinstance(q,float): #q is scalar if dimension is 1
233 q=[q]
234
235 graphicsList = []
236 T = HT0() #initial transformation
237 pPrev = [0,0,0]
238 for i in range(n):
239 # T66prev = T66
240 [A, v, rotAxis, transAxis] = JointTransformMotionSubspace(KT2.listOfJointTypes[i], q[i])
241 XL = KT2.XL(i)
242 XLHT = HT(XL[0],XL[1])
243 T = T @ XLHT @ HT(A.T,v) #A is inverse transform
244
245 p = HT2translation(T)
246 A = HT2rotationMatrix(T)
247
248 vAxis = A@np.array([0,0,0.28*L])
249 graphicsList += [GraphicsDataCylinder(pAxis=p-0.5*vAxis, vAxis=vAxis, radius=0.045*L, color=color4red)]
250 cube= GraphicsDataOrthoCubePoint(centerPoint=[0.5*L,0,0], size=[L,0.085*L,0.145*L], color=color4steelblue)
251 cube2 = MoveGraphicsData(cube, p, A)
252 graphicsList += [cube2]
253 #graphicsList += [{'type':'Line', 'data': list(p)+list(p0), 'color':color4blue}]
254 return graphicsList
255
256 nGeneric2=mbs.AddNode(NodeGenericODE2(referenceCoordinates = [0]*n,
257 initialCoordinates = [0]*n,
258 initialCoordinates_t= [0]*n,
259 numberOfODE2Coordinates = n))
260
261 mbs.AddObject(ObjectGenericODE2(nodeNumbers = [nGeneric2],
262 forceUserFunction=UFgenericODE2KT2,
263 massMatrixUserFunction=UFmassGenericODE2KT2,
264 visualization=VObjectGenericODE2(graphicsDataUserFunction=UFgraphicsKT2)))
265
266 sJointsList += [mbs.AddSensor(SensorNode(nodeNumber=nGeneric2, fileName='solution/genericNode2.txt',
267 outputVariableType=exu.OutputVariableType.Coordinates))]
268 sCases += [' KT eff']
269
270
271if useKTcpp:
272
273 if False:
274 offsetsList = exu.Vector3DList(vVec)
275 rotList = exu.Matrix3DList(Amat)
276 linkCOMs=exu.Vector3DList(listCOM)
277 linkInertiasCOM=exu.Matrix3DList(listInertiaCOM)
278 linkForces = exu.Vector3DList([[0.,0.,0.]]*n)
279 linkTorques = exu.Vector3DList([[0.,0.,0.]]*n)
280
281
282 #graphics for link and joint:
283 gLink = GraphicsDataOrthoCubePoint(centerPoint= [0.5*L,0,0], size= [L,w,w], color= color4dodgerblue)
284 gJoint = GraphicsDataCylinder([0,0,-1.25*w], [0,0,2.5*w], 0.4*w, color=color4grey)
285 gList = [[gLink,gJoint]]*n
286
287 nGenericCpp = mbs.AddNode(NodeGenericODE2(referenceCoordinates=[0.]*n,
288 initialCoordinates=[0.]*n,
289 initialCoordinates_t=[0.]*n,
290 numberOfODE2Coordinates=n))
291
292 VKT = VObjectKinematicTree(graphicsDataList = gList)
293 mbs.AddObject(ObjectKinematicTree(nodeNumber=nGenericCpp, jointTypes=[exu.JointType.RevoluteZ]*n, linkParents=np.arange(n)-1,
294 jointTransformations=rotList, jointOffsets=offsetsList, linkInertiasCOM=linkInertiasCOM,
295 linkCOMs=linkCOMs, linkMasses=listMass,
296 baseOffset = [0.,0.,0.], gravity=np.array(gravity3D), jointForceVector=[0.]*n,
297 linkForces = linkForces, linkTorques = linkTorques,
298 visualization=VKT))
299 else:
300 #use Robot class function:
301 dKT = newRobot.CreateKinematicTree(mbs)
302 nGenericCpp = dKT['nodeGeneric']
303
304 sJointsList += [mbs.AddSensor(SensorNode(nodeNumber=nGenericCpp, fileName='solution/genericNodeCpp.txt',
305 outputVariableType=exu.OutputVariableType.Coordinates))]
306
307 sCases += [' KT cpp']
308
309#exu.Print(mbs)
310mbs.Assemble()
311
312simulationSettings = exu.SimulationSettings()
313
314tEnd = 1
315h = 1e-2 #0.1
316simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
317simulationSettings.timeIntegration.endTime = tEnd
318simulationSettings.solutionSettings.solutionWritePeriod = h
319simulationSettings.timeIntegration.verboseMode = 1
320#simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
321
322simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 1 #SHOULD work with 0.9 as well
323
324useGraphics=True
325if True:
326 SC.visualizationSettings.general.autoFitScene=False
327 SC.visualizationSettings.window.renderWindowSize = [1600,1200]
328 SC.visualizationSettings.general.drawCoordinateSystem=True
329 SC.visualizationSettings.general.drawWorldBasis=True
330 SC.visualizationSettings.openGL.multiSampling=4
331 SC.visualizationSettings.nodes.showBasis = True
332 SC.visualizationSettings.nodes.basisSize = 0.5
333 if useGraphics:
334
335 exu.StartRenderer()
336 if 'renderState' in exu.sys: SC.SetRenderState(exu.sys['renderState']) #load last model view
337
338 mbs.WaitForUserToContinue() #press space to continue
339
340 mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.ExplicitEuler)
341 # mbs.SolveDynamic(simulationSettings)
342
343#u1 = mbs.GetNodeOutput(nGeneric, exu.OutputVariableType.Coordinates)
344for i in range(len(sJointsList)):
345 s = sJointsList[i]
346 qq = mbs.GetSensorValues(s)
347 exu.Print("joint angles =", qq, ", case ", sCases[i])
348
349if False: #use this to reload the solution and use SolutionViewer
350 #sol = LoadSolutionFile('coordinatesSolution.txt')
351
352 mbs.SolutionViewer() #can also be entered in IPython ...
353
354if useGraphics:
355 SC.WaitForRenderEngineStopFlag()
356 exu.StopRenderer() #safely close rendering window!
357
358
359if False:
360
361 mbs.PlotSensor(sensorNumbers=[sGeneric], components=[0])