bicycleIftommBenchmark.py
You can view and download this file on Github: bicycleIftommBenchmark.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: bicycle Iftomm benchmark example
5# https://www.iftomm-multibody.org/benchmark/problem/Uncontrolled_bicycle/
6#
7# Author: Johannes Gerstmayr
8# Date: 2021-06-22
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
15import exudyn as exu
16from exudyn.itemInterface import *
17from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
18import exudyn.graphics as graphics #only import if it does not conflict
19from exudyn.graphicsDataUtilities import *
20
21from math import sin, cos, pi
22import numpy as np
23
24SC = exu.SystemContainer()
25mbs = SC.AddSystem()
26
27
28#%%++++++++++++++++++++++++++++++++++++++++++++++++
29#coordinate system according to IFToMM:
30#note: here, wheels are rotated as local wheel axis=x, z points upwards in EXUDYN model
31# ox P2
32# oooo o
33# oooo o
34# +++ oooo o +++
35# + + ooo o +
36# + oooo + o +
37# + xP1+ + xP3+
38# + + + +
39# + + + +
40# +++ +++
41# x---------------------------x----------------------> x
42# | <- w ->
43# v z
44
45
46#parameters
47sZ = -1 #switch z coordinate compared to IFToMM description
48w = 1.02 #wheel base (distance of wheel centers)
49c = 0.08 #trail
50lam = pi/10 #steer axis tilt (rad)
51g = [0,0,9.81*sZ] #gravity in m/s^2
52
53#rear wheel R:
54rR = 0.3 #rear wheel radius
55mR = 2 #rear wheel mass
56IRxx = 0.0603 #rear wheel inertia xx = zz ; but in EXUDYN, x=rotation axis!
57IRyy = 0.12 #rear wheel inertia yy (around wheel axis)
58inertiaR = RigidBodyInertia(mass=mR, inertiaTensor=np.array([[IRyy,0,0],[0,IRxx,0],[0,0,IRxx]]))
59
60#front wheel F:
61rF = 0.35 #rear wheel radius
62mF = 3 #rear wheel mass
63IFxx = 0.1405 #rear wheel inertia xx = zz ; but in EXUDYN, x=rotation axis!
64IFyy = 0.28 #rear wheel inertia yy (around wheel axis)
65inertiaF = RigidBodyInertia(mass=mF, inertiaTensor=np.array([[IFyy,0,0],[0,IFxx,0],[0,0,IFxx]]))
66
67#rear body B:
68xB = 0.3 #COM x
69zB = -0.9*sZ #COM z
70bCOM = np.array([xB, 0, zB])
71mB = 85 #rear body (and driver) mass
72zz=-1
73inertiaB = RigidBodyInertia(mass=mB,
74 inertiaTensor=np.array([[9.2,0,2.4*zz],[0,11,0],[2.4*zz,0,2.8]]),
75 # inertiaTensor=np.diag([1,1,1]),
76 com=np.zeros(3)) #reference position = COM for this body
77 # com=bCOM)
78
79#front Handlebar H:
80xH = 0.9 #COM x
81zH = -0.7*sZ #COM z
82hCOM = np.array([xH, 0, zH])
83mH = 4 #handle bar mass
84inertiaH = RigidBodyInertia(mass=mH,
85 inertiaTensor=np.array([[0.05892, 0, -0.00756*zz],[0,0.06,0],[-0.00756*zz, 0, 0.00708]]),
86 # inertiaTensor=np.diag([1,1,1]),
87 com=np.zeros(3)) #reference position = COM for this body
88 # com=hCOM)
89
90#geometrical parameters for joints
91P1 = np.array([0,0,-0.3*sZ])
92P2 = np.array([0.82188470506, 0, -0.85595086466*sZ])
93P3 = np.array([w, 0, -0.35*sZ])
94
95
96#stable velocity limits according to linear theory:
97vMin = 4.29238253634111
98vMax = 6.02426201538837
99
100maneuver = 'M1'
101if maneuver == 'M1':
102 vX0 = 4. #initial forward velocity in x-direction
103 omegaX0 = 0.05 #initial roll velocity around x axis
104elif maneuver == 'M2':
105 vX0 = 4.6 #initial forward velocity in x-direction
106 omegaX0 = 0.5 #initial roll velocity around x axis
107elif maneuver == 'M3':
108 vX0 = 8 #initial forward velocity in x-direction
109 omegaX0 = 0.05 #initial roll velocity around x axis
110
111omegaRy0 = -vX0/rR*sZ #initial angular velocity of rear wheel
112omegaFy0 = -vX0/rF*sZ #initial angular velocity of front wheel
113
114#%%++++++++++++++++++++++++++++++++++++++++++++++++
115#visualization:
116dY = 0.02
117#graphicsFrame = graphics.Brick(centerPoint=[0,0,0],size=[dFoot*1.1,0.7*rFoot,0.7*rFoot], color=graphics.color.lightred)
118graphicsR = graphics.Cylinder(pAxis=[-1*dY,0,0], vAxis=[dY*2,0,0], radius=rR, color=graphics.color.steelblue, nTiles=4)
119graphicsF = graphics.Cylinder(pAxis=[-1*dY,0,0], vAxis=[dY*2,0,0], radius=rF, color=graphics.color.steelblue, nTiles=4)
120graphicsB = graphics.Cylinder(pAxis=P1-bCOM, vAxis=P2-P1, radius=dY*1.5, color=graphics.color.lightred)
121graphicsB2 = graphics.Sphere(point=[0,0,0], radius=3*dY, color=graphics.color.lightgrey)
122graphicsH = graphics.Cylinder(pAxis=P3-hCOM, vAxis=P2-P3, radius=dY*1.3, color=graphics.color.lightgreen)
123
124#option to track motion of bicycle
125if True:
126 #add user function to track bicycle frame
127 def UFgraphics(mbs, objectNum):
128 n = mbs.variables['nTrackNode']
129 p = mbs.GetNodeOutput(n,exu.OutputVariableType.Position,
130 configuration=exu.ConfigurationType.Visualization)
131 rs=SC.GetRenderState()
132 A = np.array(rs['modelRotation'])
133 p = A.T @ p
134 rs['centerPoint']=[p[0],p[1],p[2]]
135 SC.SetRenderState(rs)
136 return []
137
138 #add object with graphics user function
139 oGround2 = mbs.AddObject(ObjectGround(visualization=
140 VObjectGround(graphicsDataUserFunction=UFgraphics)))
141#add rigid bodies
142#rear wheel
143resultR = mbs.CreateRigidBody(
144 referencePosition = P1,
145 referenceRotationMatrix = RotationMatrixZ(np.pi*0.5),
146 initialVelocity = [vX0, omegaX0*P1[2]*sZ, 0],
147 initialAngularVelocity = [omegaX0, omegaRy0, 0],
148 inertia = inertiaR,
149 gravity = g,
150 graphicsDataList = [graphicsR],
151 returnDict = True)
152
153nR, bR = resultR['nodeNumber'], resultR['bodyNumber']
154
155mbs.variables['nTrackNode'] = nR #node to be tracked
156
157#front wheel
158resultF = mbs.CreateRigidBody(
159 referencePosition = P3,
160 referenceRotationMatrix = RotationMatrixZ(pi*0.5),
161 initialVelocity = [vX0, omegaX0*P3[2]*sZ, 0],
162 initialAngularVelocity = [omegaX0, omegaFy0, 0],
163 inertia = inertiaF,
164 gravity = g,
165 nodeType = exu.NodeType.RotationEulerParameters,
166 graphicsDataList = [graphicsF],
167 returnDict = True
168)
169nF, bF = resultF['nodeNumber'], resultF['bodyNumber']
170
171#read body
172resultB = mbs.CreateRigidBody(
173 referencePosition = bCOM,
174 initialVelocity = [vX0, omegaX0*bCOM[2]*sZ, 0],
175 initialAngularVelocity = [omegaX0, 0, 0],
176 inertia = inertiaB,
177 gravity = g,
178 nodeType = exu.NodeType.RotationEulerParameters,
179 graphicsDataList = [graphicsB, graphicsB2],
180 returnDict = True
181)
182nB, bB = resultB['nodeNumber'], resultB['bodyNumber']
183
184#handle
185resultH = mbs.CreateRigidBody(
186 referencePosition = hCOM,
187 initialVelocity = [vX0, omegaX0*hCOM[2]*sZ, 0],
188 initialAngularVelocity = [omegaX0, 0, 0],
189 inertia = inertiaH,
190 gravity = g,
191 nodeType = exu.NodeType.RotationEulerParameters,
192 graphicsDataList = [graphicsH],
193 returnDict = True
194)
195nH, bH = resultH['nodeNumber'], resultH['bodyNumber']
196
197
198#%%++++++++++++++++++++++++++++++++++++++++++++++++
199#ground body and marker
200gGround = graphics.CheckerBoard(point=[0,0,0], size=50, nTiles=64)
201oGround = mbs.CreateGround(graphicsDataList=[gGround])
202markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
203
204markerR = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bR, localPosition=[0,0,0]))
205markerF = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bF, localPosition=[0,0,0]))
206markerB1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bB, localPosition=P1-bCOM))
207
208sMarkerR = mbs.AddSensor(SensorMarker(markerNumber=markerR, outputVariableType=exu.OutputVariableType.Position))
209sMarkerB1= mbs.AddSensor(SensorMarker(markerNumber=markerB1,outputVariableType=exu.OutputVariableType.Position))
210
211#%%++++++++++++++++++++++++++++++++++++++++++++++++
212#add joints:
213useJoints = True
214if useJoints:
215 oJointRW = mbs.CreateRevoluteJoint(bodyNumbers=[bR, bB], position=P1, axis=[0,1,0],
216 axisRadius=0.5*dY, axisLength=5*dY)
217 oJointFW = mbs.CreateRevoluteJoint(bodyNumbers=[bF, bH], position=P3, axis=[0,1,0],
218 axisRadius=0.5*dY, axisLength=5*dY)
219 oJointSteer = mbs.CreateRevoluteJoint(bodyNumbers=[bB, bH],
220 position=P2-bCOM, useGlobalFrame=False,
221 axis=RotationMatrixY(-lam) @ [0,0,1],
222 axisRadius=0.5*dY, axisLength=5*dY)[0]
223#%%++++++++++++++++++++++++++++++++++++++++++++++++
224#add 'rolling disc' for wheels:
225cStiffness = 5e4*10 #spring stiffness: 50N==>F/k = u = 0.001m (penetration)
226cDamping = cStiffness*0.05*0.1 #think on a one-mass spring damper
227nGenericR = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
228if False:
229 oRollingR=mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[markerGround, markerR],
230 nodeNumber = nGenericR,
231 discRadius=rR,
232 planeNormal=[0,0,1],
233 dryFriction=[0.8,0.8],
234 dryFrictionProportionalZone=1e-2,
235 rollingFrictionViscous=0.,
236 contactStiffness=cStiffness,
237 contactDamping=cDamping,
238 #activeConnector = False, #set to false to deactivated
239 visualization=VObjectConnectorRollingDiscPenalty(show=True,
240 discWidth=dY, color=graphics.color.blue)))
241
242 nGenericF = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
243 oRollingF=mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[markerGround, markerF],
244 nodeNumber = nGenericF,
245 discRadius=rF,
246 planeNormal=[0,0,1],
247 dryFriction=[0.8,0.8],
248 dryFrictionProportionalZone=1e-2,
249 rollingFrictionViscous=0.,
250 contactStiffness=cStiffness,
251 contactDamping=cDamping,
252 #activeConnector = False, #set to false to deactivated
253 visualization=VObjectConnectorRollingDiscPenalty(show=True, discWidth=dY, color=graphics.color.blue)))
254else:
255 if True:
256 oRollingR=mbs.AddObject(ObjectJointRollingDisc(markerNumbers=[markerGround, markerR],
257 discRadius=rR,
258 visualization=VObjectJointRollingDisc(show=True, discWidth=dY, color=graphics.color.blue)))
259
260 oRollingF=mbs.AddObject(ObjectJointRollingDisc(markerNumbers=[markerGround, markerF],
261 discRadius=rF,
262 visualization=VObjectJointRollingDisc(show=True, discWidth=dY, color=graphics.color.blue)))
263
264
265
266#%%++++++++++++++++++++++++++++++++++++++++++++++++
267#add sensors
268addSensors = True
269if addSensors:
270 sForwardVel = mbs.AddSensor(SensorBody(bodyNumber=bB, fileName='solution/bicycleBvelLocal.txt',
271 localPosition=P1-bCOM,
272 outputVariableType=exu.OutputVariableType.VelocityLocal))
273
274 sBAngVelLocal = mbs.AddSensor(SensorBody(bodyNumber=bB, fileName='solution/bicycleBangVelLocal.txt',
275 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
276 sBrot = mbs.AddSensor(SensorBody(bodyNumber=bB, fileName='solution/bicycleBrot.txt',
277 outputVariableType=exu.OutputVariableType.Rotation))
278
279
280 bodies = [bB, bH, bR, bF]
281 massBodies = [mB, mH, mR, mF]
282 inertiaBodies = [inertiaB.inertiaTensor,
283 inertiaH.inertiaTensor,
284 inertiaR.inertiaTensor,
285 inertiaF.inertiaTensor]
286
287 nBodies = len(bodies)
288 sList = []
289 for b in bodies:
290 sPosCOM = mbs.AddSensor(SensorBody(bodyNumber=b, writeToFile=False,
291 outputVariableType=exu.OutputVariableType.Position))
292 sVelCOM = mbs.AddSensor(SensorBody(bodyNumber=b, writeToFile=False,
293 outputVariableType=exu.OutputVariableType.Velocity))
294 sAngVelLocal = mbs.AddSensor(SensorBody(bodyNumber=b, writeToFile=False,
295 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
296
297 sList += [sPosCOM,sVelCOM,sAngVelLocal]
298
299 if useJoints:
300 sSteerAngle = mbs.AddSensor(SensorObject(objectNumber=oJointSteer, fileName='solution/bicycleSteerAngle.txt',
301 outputVariableType=exu.OutputVariableType.Rotation))
302 sSteerVel = mbs.AddSensor(SensorObject(objectNumber=oJointSteer, fileName='solution/bicycleSteerVelocity.txt',
303 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
304
305
306 #create user joint for kinetic and potential energy
307 def UFsensorEnergy(mbs, t, sensorNumbers, factors, configuration):
308 E = 0
309 P = 0
310 for i in range(nBodies):
311 pos = mbs.GetSensorValues(sensorNumbers[i*3+0])
312 vel = mbs.GetSensorValues(sensorNumbers[i*3+1]) #vel
313 omega = mbs.GetSensorValues(sensorNumbers[i*3+2]) #ang vel local
314 E += 0.5 * NormL2(vel)**2 * massBodies[i]
315 E += 0.5 * np.array(omega) @ inertiaBodies[i] @ omega
316
317 P -= np.dot(g,pos)*massBodies[i]
318 return [P, E, E+P] #return potential, kinetic and total mechanical energy
319
320 sEnergy = mbs.AddSensor(SensorUserFunction(sensorNumbers=sList, #factors=[180/pi],
321 fileName='solution/sensorKineticPotentialEnergy.txt',
322 sensorUserFunction=UFsensorEnergy))
323
324 def UFsensorResults(mbs, t, sensorNumbers, factors, configuration):
325 energy = mbs.GetSensorValues(sensorNumbers[0])
326 forwardVel = mbs.GetSensorValues(sensorNumbers[1])
327 angVelLocalB = mbs.GetSensorValues(sensorNumbers[2])
328 rotB = mbs.GetSensorValues(sensorNumbers[3])
329 steerAngle = mbs.GetSensorValues(sensorNumbers[4])
330 steerVel = mbs.GetSensorValues(sensorNumbers[5])
331 return [rotB[0], angVelLocalB[0], forwardVel[0], energy[0], energy[1], energy[2], -steerAngle[2], -steerVel[2]] #return kinetic, potential and total mechanical energy
332
333 # 1=roll angle, 2=roll angular velocity, 3=forward speed, 4=potential energy, 5=kinetic energy, 6=mechanical energy, 7=steer angle, and 8=steer velocity
334 sResults = mbs.AddSensor(SensorUserFunction(sensorNumbers=[sEnergy,sForwardVel,sBAngVelLocal,sBrot, sSteerAngle, sSteerVel],
335 fileName='solution/sensorResults'+maneuver+'.txt',
336 sensorUserFunction=UFsensorResults))
337
338#%%++++++++++++++++++++++++++++++++++++++++++++++++
339#simulate:
340mbs.Assemble()
341
342pR = mbs.GetSensorValues(sMarkerR)
343pB1 = mbs.GetSensorValues(sMarkerB1)
344print("pR=",pR)
345print("pB1=",pB1)
346simulationSettings = exu.SimulationSettings() #takes currently set values or default values
347
348tEnd = 5
349h=0.001 #use small step size to detext contact switching
350
351simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
352simulationSettings.timeIntegration.endTime = tEnd
353simulationSettings.solutionSettings.writeSolutionToFile= False #set False for CPU performance measurement
354simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
355simulationSettings.solutionSettings.outputPrecision = 16
356
357simulationSettings.timeIntegration.verboseMode = 1
358#simulationSettings.linearSolverSettings.ignoreSingularJacobian = True
359
360# simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
361# simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
362simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.7
363simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
364simulationSettings.timeIntegration.newton.useModifiedNewton = True
365
366SC.visualizationSettings.nodes.show = True
367SC.visualizationSettings.nodes.drawNodesAsPoint = False
368SC.visualizationSettings.nodes.showBasis = True
369SC.visualizationSettings.nodes.basisSize = 0.015
370
371if False: #record animation frames:
372 SC.visualizationSettings.exportImages.saveImageFileName = "animation/frame"
373 SC.visualizationSettings.window.renderWindowSize=[1600,1024]
374 SC.visualizationSettings.openGL.multiSampling = 4
375 simulationSettings.solutionSettings.recordImagesInterval = 0.02
376
377SC.visualizationSettings.general.autoFitScene = False #use loaded render state
378useGraphics = True
379if useGraphics:
380 exu.StartRenderer()
381 if 'renderState' in exu.sys:
382 SC.SetRenderState(exu.sys[ 'renderState' ])
383 mbs.WaitForUserToContinue()
384
385mbs.SolveDynamic(simulationSettings, solverType=exu.DynamicSolverType.TrapezoidalIndex2)
386#mbs.SolveDynamic(simulationSettings, showHints=True)
387
388
389#%%+++++++++++++++++++++++++++++
390if useGraphics:
391 SC.WaitForRenderEngineStopFlag()
392 exu.StopRenderer() #safely close rendering window!
393
394#%%++++++++++++++++++++++++++++++++++++++++++++++q+++++++
395if addSensors:
396 #plot results
397
398
399
400 import matplotlib.pyplot as plt
401 import matplotlib.ticker as ticker
402 plt.close('all')
403
404
405 # mbs.PlotSensor(sensorNumbers=[sBpos,sBpos,sBpos], components=[0,1,2])
406 #plt.figure('lateral position')
407 #mbs.PlotSensor(sensorNumbers=[sBpos], components=[1])
408
409 plt.figure('forward velocity')
410 mbs.PlotSensor(sensorNumbers=[sForwardVel], components=[0])
411 # mbs.PlotSensor(sensorNumbers=[sBvelLocal,sBvelLocal,sBvelLocal], components=[0,1,2])
412
413 # plt.figure('local ang velocities')
414 # mbs.PlotSensor(sensorNumbers=[sBAngVelLocal,sBAngVelLocal,sBAngVelLocal], components=[0,1,2])
415 # if False:
416 # import matplotlib.pyplot as plt
417 # import matplotlib.ticker as ticker
418
419 # 1=roll angle, 2=roll angular velocity, 3=forward speed, 4=potential energy, 5=kinetic energy, 6=mechanical energy, 7=steer angle, and 8=steer velocity
420 data = np.loadtxt('solution/uncontrolledBicycleGonzalez.txt')#, comments='#', delimiter='')
421 plt.plot(data[:,0], data[:,9], 'b:',label='')
422
423 data2 = np.loadtxt('solution/uncontrolledBicycleSanjurjo.txt')#, comments='#', delimiter='')
424 plt.plot(data2[:,0], data2[:,3+8], 'g:',label='')
425
426 plt.figure('steer vel')
427 mbs.PlotSensor(sensorNumbers=[sSteerVel], components=[2])
428 plt.plot(data2[:,0], -data2[:,8+8], 'g:',label='')
429
430 plt.figure('steer ang')
431 mbs.PlotSensor(sensorNumbers=[sSteerAngle], components=[2])
432 plt.plot(data2[:,0], -data2[:,7+8], 'g:',label='')
433
434 plt.figure('roll')
435 mbs.PlotSensor(sensorNumbers=[sBrot], components=[0])
436 plt.plot(data2[:,0], data2[:,1+8], 'g:',label='')
437
438 plt.figure('roll ang vel')
439 mbs.PlotSensor(sensorNumbers=[sBAngVelLocal], components=[0])
440 plt.plot(data2[:,0], data2[:,2+8], 'g:',label='')
441
442
443 plt.figure('potential energy')
444 mbs.PlotSensor(sensorNumbers=[sEnergy], components=[0])
445 plt.plot(data2[:,0], data2[:,4+8], 'g:',label='')
446
447 plt.figure('kinetic energy')
448 mbs.PlotSensor(sensorNumbers=[sEnergy], components=[1])
449 plt.plot(data2[:,0], data2[:,5+8], 'g:',label='')
450
451 plt.figure('total energy')
452 mbs.PlotSensor(sensorNumbers=[sEnergy], components=[2])
453
454 dataE = np.loadtxt('solution/sensorKineticPotentialEnergy.txt', comments='#', delimiter=',')
455 performance = 100*(max(dataE[:,3]) - min(dataE[:,3])) / dataE[0,3]
456 print("performance = ", performance, "(must by < 1e-3)")
457
458 #CPU performance with 20 seconds simulation time, maneuver 2
459 #performance = 0.000915 < 0.001: max h=0.014; CPU time = 0.596 seconds on Intel Core i9
460 #reference solution computed with:
461 # performance = 6.423e-06: max h=0.001; CPU time = 5.5935 seconds on Intel Core i9
462
463
464#%%+++++++++++++++++
465#merge result files for IFToMM
466if True:
467 dataM1 = np.loadtxt('solution/sensorResultsM1.txt', comments='#', delimiter=',')
468 dataM2 = np.loadtxt('solution/sensorResultsM2.txt', comments='#', delimiter=',')
469 dataM3 = np.loadtxt('solution/sensorResultsM3.txt', comments='#', delimiter=',')
470
471 data = np.hstack((dataM1,dataM2[:,1:],dataM3[:,1:]))
472 np.savetxt('solution/bicycleResultsIFToMM.txt', data, fmt='%1.15e')
473
474# benchmark results:
475# 6.423e-06
476# 5.5935
477# Intel(R) Core(TM) i9-7940X CPU @ 3.10GHz
478# Simulated using C++/Python library EXUDYN, see https://github.com/jgerstmayr/EXUDYN.
479# Solved using implicit trapezoidal rule (energy conserving) with Index 2 constraint reduction, using redundant coordinate formulation. Rigid bodies are modeled with Euler parameters.
480# With a larger step size of 0.014 we still obtain a performance <0.001, but have CPU time of 0.596 seconds.