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