serialRobotTest.py

You can view and download this file on Github: serialRobotTest.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test Example of a serial robot with redundant coordinates
  5#           NOTE: more efficient version in serialRobotTSD.py and serialRobotKinematicTree.py using TorsionalSpringDamper
  6#
  7# Author:   Johannes Gerstmayr
  8# Date:     2020-02-16
  9# Revised:  2021-07-09
 10#
 11# 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.
 12#
 13#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 14
 15import exudyn as exu
 16from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 17import exudyn.graphics as graphics #only import if it does not conflict
 18from exudyn.rigidBodyUtilities import *
 19from exudyn.robotics import *
 20from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration
 21
 22import numpy as np
 23from numpy import linalg as LA
 24
 25useGraphics = True #without test
 26#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 27#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 28try: #only if called from test suite
 29    from modelUnitTests import exudynTestGlobals #for globally storing test results
 30    useGraphics = exudynTestGlobals.useGraphics
 31except:
 32    class ExudynTestGlobals:
 33        pass
 34    exudynTestGlobals = ExudynTestGlobals()
 35#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 36
 37SC = exu.SystemContainer()
 38mbs = SC.AddSystem()
 39
 40#useGraphics = False
 41if useGraphics:
 42    sensorWriteToFile = True
 43else:
 44    sensorWriteToFile = False
 45
 46jointWidth=0.1
 47jointRadius=0.06
 48linkWidth=0.1
 49
 50graphicsBaseList = [graphics.Brick([0,0,-0.15], [0.4,0.4,0.1], graphics.color.grey)]
 51graphicsBaseList +=[graphics.Cylinder([0,0,0], [0.5,0,0], 0.0025, graphics.color.red)]
 52graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0.5,0], 0.0025, graphics.color.green)]
 53graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0,0.5], 0.0025, graphics.color.blue)]
 54graphicsBaseList +=[graphics.Cylinder([0,0,-jointWidth], [0,0,jointWidth], linkWidth*0.5, graphics.colorList[0])] #belongs to first body
 55
 56ty = 0.03
 57tz = 0.04
 58zOff = -0.05
 59toolSize= [0.05,0.5*ty,0.06]
 60graphicsToolList = [graphics.Cylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=graphics.color.red)]
 61graphicsToolList+= [graphics.Brick([0,ty,1.5*tz+zOff], toolSize, graphics.color.grey)]
 62graphicsToolList+= [graphics.Brick([0,-ty,1.5*tz+zOff], toolSize, graphics.color.grey)]
 63
 64
 65#changed to new robot structure July 2021:
 66newRobot = Robot(gravity=[0,0,9.81],
 67                 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
 68                 tool = RobotTool(HT=HTtranslate([0,0,0.1]), visualization=VRobotTool(graphicsData=graphicsToolList)),
 69                 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
 70
 71newRobot.AddLink(RobotLink(mass=20, COM=[0,0,0], inertia=np.diag([1e-8,0.35,1e-8]), localHT = StdDH2HT([0,0,0,np.pi/2]), visualization=VRobotLink(linkColor=graphics.colorList[0])))
 72newRobot.AddLink(RobotLink(mass=17.4, COM=[-0.3638, 0.006, 0.2275], inertia=np.diag([0.13,0.524,0.539]), localHT = StdDH2HT([0,0,0.4318,0]), visualization=VRobotLink(linkColor=graphics.colorList[1])))
 73newRobot.AddLink(RobotLink(mass=4.8, COM=[-0.0203,-0.0141,0.07], inertia=np.diag([0.066,0.086,0.0125]), localHT = StdDH2HT([0,0.15,0.0203,-np.pi/2]), visualization=VRobotLink(linkColor=graphics.colorList[2])))
 74newRobot.AddLink(RobotLink(mass=0.82, COM=[0,0.019,0], inertia=np.diag([0.0018,0.0013,0.0018]), localHT = StdDH2HT([0,0.4318,0,np.pi/2]), visualization=VRobotLink(linkColor=graphics.colorList[3])))
 75newRobot.AddLink(RobotLink(mass=0.34, COM=[0,0,0], inertia=np.diag([0.0003,0.0004,0.0003]), localHT = StdDH2HT([0,0,0,-np.pi/2]), visualization=VRobotLink(linkColor=graphics.colorList[4])))
 76newRobot.AddLink(RobotLink(mass=0.09, COM=[0,0,0.032], inertia=np.diag([0.00015,0.00015,4e-5]), localHT = StdDH2HT([0,0,0,0]), visualization=VRobotLink(linkColor=graphics.colorList[5])))
 77
 78
 79# newRobot = Robot(gravity=[0,0,-9.81], referenceConfiguration = [])
 80# newRobot.BuildFromDictionary(myRobot) #this allows conversion from old structure
 81
 82#assumption, as the bodies in the mbs have their COM at the reference position
 83#for link in robot['links']:
 84#    link['COM']=[0,0,0]
 85
 86#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 87#configurations and trajectory
 88q0 = [0,0,0,0,0,0] #zero angle configuration
 89
 90q1 = [0,       np.pi/8, np.pi*0.25, 0,np.pi/8,0] #configuration 1
 91q2 = [np.pi/2,-np.pi/8,-np.pi*0.125, 0,np.pi/4,0] #configuration 2
 92#q2 = [np.pi*0.45,-np.pi*0.35,-np.pi*0.25, np.pi*0.10,np.pi*0.2,np.pi*0.3] #configuration 2
 93#q2 = [pi/2,-pi/8,-pi*0.125,0,pi/4,pi/2] #configuration 2
 94
 95trajectory=Trajectory(q0, 0)
 96if False: #tests for static torque compensation
 97    #trajectory.Add(ProfileConstantAcceleration(q0, 1)) #V1.2.37: torques at tEnd= 0.3773259482700045
 98    trajectory.Add(ProfileConstantAcceleration(q2, 0.25))
 99    trajectory.Add(ProfileConstantAcceleration(q2, 0.75))
100else: #standard test case:
101    trajectory.Add(ProfileConstantAcceleration(q1, 0.25))
102    trajectory.Add(ProfileConstantAcceleration(q2, 0.25))
103    trajectory.Add(ProfileConstantAcceleration(q0, 0.5 ))
104    trajectory.Add(ProfileConstantAcceleration(q0, 1e6))
105
106#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
107#test robot model
108#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
109#control parameters, per joint:
110Pcontrol = np.array([40000, 40000, 40000, 100, 100, 10])
111Dcontrol = np.array([400,   400,   100,   1,   1,   0.05])  #last value 0.05 gives less oscillations as compared to earlier values of 0.1!
112# Pcontrol = 0.01*Pcontrol #soft behavior
113
114
115#++++++++++++++++++++++++++++++++++++++++++++++++
116#base, graphics, object and marker:
117graphicsBaseList = [graphics.Brick([0,0,-0.2], [0.4,0.4,0.1], graphics.color.grey)]
118graphicsBaseList +=[graphics.Cylinder([0,0,0], [0.5,0,0], 0.0025, graphics.color.red)]
119graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0.5,0], 0.0025, graphics.color.green)]
120graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0,0.5], 0.0025, graphics.color.blue)]
121#oGround = mbs.AddObject(ObjectGround(referencePosition=list(HT2translation(Tcurrent)),
122objectGround = mbs.AddObject(ObjectGround(referencePosition=HT2translation(newRobot.GetBaseHT()),
123                                     visualization=VObjectGround(graphicsData=graphicsBaseList)))
124
125#baseMarker; could also be a moving base!
126baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
127
128
129
130#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
131#build mbs robot model:
132robotDict = newRobot.CreateRedundantCoordinateMBS(mbs, baseMarker=baseMarker)
133
134#   !!!!!IMPORTANT!!!!!:
135jointList = robotDict['jointList'] #must be stored there for the load user function
136
137unitTorques0 = robotDict['unitTorque0List'] #(left body)
138unitTorques1 = robotDict['unitTorque1List'] #(right body)
139
140loadList0 = robotDict['jointTorque0List'] #(left body)
141loadList1 = robotDict['jointTorque1List'] #(right body)
142#print(loadList0, loadList1)
143#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
144#control robot
145compensateStaticTorques = True
146def ComputeMBSstaticRobotTorques(newRobot):
147    q=[]
148    for joint in jointList:
149        q += [mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2]] #z-rotation
150    HT=newRobot.JointHT(q)
151    return newRobot.StaticTorques(HT)
152
153#user function which is called only once per step, speeds up simulation drastically
154def PreStepUF(mbs, t):
155    if compensateStaticTorques:
156        staticTorques = ComputeMBSstaticRobotTorques(newRobot)
157    else:
158        staticTorques = np.zeros(len(jointList))
159    #compute load for joint number
160    for i in range(len(jointList)):
161        joint = i
162        phi = mbs.GetObjectOutput(jointList[joint], exu.OutputVariableType.Rotation)[2] #z-rotation
163        omega = mbs.GetObjectOutput(jointList[joint], exu.OutputVariableType.AngularVelocityLocal)[2] #z-angular velocity
164
165        #[u,v,a] = MotionInterpolator(t, robotTrajectory, joint) #OLD, until V1.1.135
166        [u,v,a] = trajectory.EvaluateCoordinate(t, joint)
167
168
169        torque = -(Pcontrol[joint]*(phi-u) + Dcontrol[joint]*(omega-v)) #negative sign in feedback control!
170        # torque = (Pcontrol[joint]*(phi+u) + Dcontrol[joint]*(omega+v)) #until V1.2.; but static Torque compensation is wrong in this case!
171        torque -= staticTorques[joint] #add static torque compensation
172
173        load0 = torque * unitTorques0[i] #includes sign and correct unit-torque vector
174        load1 = torque * unitTorques1[i] #includes sign and correct unit-torque vector
175
176    #     #write updated torque to joint loads, applied to left and right body
177        mbs.SetLoadParameter(loadList0[i], 'loadVector', list(load0))
178        mbs.SetLoadParameter(loadList1[i], 'loadVector', list(load1))
179
180    return True
181
182mbs.SetPreStepUserFunction(PreStepUF)
183
184
185
186#add sensors:
187sJointRot = []
188sJointAngVel = []
189sJointTorque = []
190cnt = 0
191for jointLink in jointList:
192    sJointRot += [mbs.AddSensor(SensorObject(objectNumber=jointLink,
193                               storeInternal=True,#fileName="solution/joint" + str(cnt) + "Rot.txt",
194                               outputVariableType=exu.OutputVariableType.Rotation,
195                               writeToFile = sensorWriteToFile))]
196    sJointAngVel += [mbs.AddSensor(SensorObject(objectNumber=jointLink,
197                               storeInternal=True,#fileName="solution/joint" + str(cnt) + "AngVel.txt",
198                               outputVariableType=exu.OutputVariableType.AngularVelocityLocal,
199                               writeToFile = sensorWriteToFile))]
200    cnt+=1
201
202cnt = 0
203for load0 in robotDict['jointTorque0List']:
204    sJointTorque += [mbs.AddSensor(SensorLoad(loadNumber=load0,storeInternal=True,# fileName="solution/jointTorque" + str(cnt) + ".txt",
205                                              writeToFile = sensorWriteToFile))]
206    cnt+=1
207
208
209
210mbs.Assemble()
211#mbs.systemData.Info()
212
213SC.visualizationSettings.connectors.showJointAxes = True
214SC.visualizationSettings.connectors.jointAxesLength = 0.02
215SC.visualizationSettings.connectors.jointAxesRadius = 0.002
216
217SC.visualizationSettings.nodes.showBasis = True
218SC.visualizationSettings.nodes.basisSize = 0.1
219SC.visualizationSettings.loads.show = False
220
221SC.visualizationSettings.openGL.multiSampling=4
222
223tEnd = 0.2 #0.2 for testing
224h = 0.001
225
226if useGraphics:
227    tEnd = 0.2
228    #tEnd = 1 #shows exactly static torques ComputeMBSstaticRobotTorques(newRobot) and desired angles (q2) at end
229
230#mbs.WaitForUserToContinue()
231simulationSettings = exu.SimulationSettings() #takes currently set values or default values
232
233simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
234simulationSettings.timeIntegration.endTime = tEnd
235simulationSettings.solutionSettings.solutionWritePeriod = h
236simulationSettings.solutionSettings.sensorsWritePeriod = h
237simulationSettings.solutionSettings.writeSolutionToFile = useGraphics
238# simulationSettings.timeIntegration.simulateInRealtime = True
239# simulationSettings.timeIntegration.realtimeFactor = 0.25
240
241simulationSettings.timeIntegration.verboseMode = 1
242simulationSettings.displayComputationTime = False
243simulationSettings.displayStatistics = False
244simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
245
246#simulationSettings.timeIntegration.newton.useModifiedNewton = True
247simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = False
248simulationSettings.timeIntegration.generalizedAlpha.useNewmark = simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints
249simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 #0.6 works well
250
251simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
252
253mbs.SolveDynamic(simulationSettings)
254
255if useGraphics:
256    SC.visualizationSettings.general.autoFitScene=False
257    exu.StartRenderer()
258    if 'renderState' in exu.sys:
259        SC.SetRenderState(exu.sys['renderState'])
260
261
262    mbs.SolutionViewer()
263    exu.StopRenderer()
264
265lastRenderState = SC.GetRenderState() #store model view
266
267#compute final torques:
268measuredTorques=[]
269
270for cnt, sensorNumber in enumerate(sJointTorque):
271    if useGraphics:
272        exu.Print('sensor torque',cnt, '=', mbs.GetSensorValues(sensorNumber))
273    measuredTorques += [1e-2*mbs.GetSensorValues(sensorNumber)[2]]
274
275if useGraphics:
276    for cnt, sensorNumber in enumerate(sJointRot):
277        exu.Print('sensor rot ',cnt, '=', mbs.GetSensorValues(sensorNumber))
278
279
280
281exu.Print("torques at tEnd=", VSum(measuredTorques))
282
283#add larger test tolerance for 32/64bits difference
284exudynTestGlobals.testError = (VSum(measuredTorques) - 0.7681856909852399)  #until 2022-04-21: 7680031232063571; until 2021-09-10: 76.8003123206452; until 2021-08-19 (changed robotics.py): 76.80031232091771; old controller: 77.12176106978085) #OLDER results: up to 2021-06-28: 0.7712176106955341; 2020-08-25: 77.13193176752571 (32bits),   2020-08-24: (64bits)77.13193176846507
285exudynTestGlobals.testResult = VSum(measuredTorques)
286
287#exu.Print('error=', exudynTestGlobals.testError)
288
289if useGraphics:
290
291
292    mbs.PlotSensor(sJointTorque, components=2, closeAll=True, yLabel='joint torques (Nm)', title='joint torques')
293
294    mbs.PlotSensor(sJointRot, components=2, yLabel='joint angles (rad)', title='joint angles')
295
296    #V1.2.40, P39: with D[-1]=0.05: since 2022-04-22:
297    # sensor torque 0 = [  0.           0.         -12.68901871]
298    # sensor torque 1 = [-0.         -0.         76.45031947]
299    # sensor torque 2 = [-0.         -0.         12.97010176]
300    # sensor torque 3 = [ 0.00000000e+00  0.00000000e+00 -8.56924875e-05]
301    # sensor torque 4 = [-0.         -0.          0.08725323]
302    # sensor torque 5 = [ 0.0000000e+00  0.0000000e+00 -9.5769297e-07]
303    # sensor rot  0 = [-1.04899069e-15  3.05694259e-19 -3.19543456e-04]
304    # sensor rot  1 = [-4.57411886e-14 -1.77599905e-14  3.63829923e-01]
305    # sensor rot  2 = [ 4.68252006e-14 -1.78150803e-14  7.25569701e-01]
306    # sensor rot  3 = [-1.07990283e-13  1.95010674e-13  2.42757634e-07]
307    # sensor rot  4 = [ 1.07852866e-13 -2.06184741e-13  3.63658987e-01]
308    # sensor rot  5 = [ 1.84235149e-16 -6.26054764e-13 -5.07549126e-08]
309    # torques at tEnd= 0.7681856909852399
310
311    #V1.2.40, P39: with D[-1]=0.1:
312    # sensor torque 0 = [  0.           0.         -12.68901812]
313    # sensor torque 1 = [-0.        -0.        76.4503195]
314    # sensor torque 2 = [-0.         -0.         12.97010177]
315    # sensor torque 3 = [ 0.0000000e+00  0.0000000e+00 -8.9097007e-05]
316    # sensor torque 4 = [-0.         -0.          0.08725323]
317    # sensor torque 5 = [-0.00000000e+00 -0.00000000e+00  1.09612342e-05]
318    # sensor rot  0 = [-1.04899069e-15  3.32799313e-19 -3.19543454e-04]
319    # sensor rot  1 = [-4.59632332e-14 -1.77794520e-14  3.63829923e-01]
320    # sensor rot  2 = [ 4.71643413e-14 -1.78734434e-14  7.25569701e-01]
321    # sensor rot  3 = [-1.07609037e-13  1.95343741e-13  2.42965135e-07]
322    # sensor rot  4 = [ 1.07471637e-13 -2.06279100e-13  3.63658987e-01]
323    # sensor rot  5 = [ 1.84106879e-16 -6.25874352e-13  8.28833899e-08]
324    # torques at tEnd= 0.7681857824086907
325
326    #V1.2.37, P37: ==> uses wrong static torque compensation
327    # sensor torque 0 = [  0.           0.         -12.67938332]
328    # sensor torque 1 = [-0.         -0.         76.42613004]
329    # sensor torque 2 = [-0.         -0.         12.96641046]
330    # sensor torque 3 = [ 0.00000000e+00  0.00000000e+00 -8.93114211e-05]
331    # sensor torque 4 = [-0.         -0.          0.08723331]
332    # sensor torque 5 = [-0.0000000e+00 -0.0000000e+00  1.1143982e-05]
333    # sensor rot  0 = [ 1.60812265e-16 -2.71050543e-20  3.19257214e-04]
334    # sensor rot  1 = [ 5.06049354e-14 -1.91882065e-14 -3.63432530e-01]
335    # sensor rot  2 = [-4.07000942e-14  3.58791681e-14 -7.25182770e-01]
336    # sensor rot  3 = [ 9.91554982e-13  4.63740157e-13 -2.41708605e-07]
337    # sensor rot  4 = [-9.26617234e-13  3.52539928e-13 -3.63103888e-01]
338    # sensor rot  5 = [ 9.21913969e-18  3.74700271e-16 -8.34127775e-08]
339    # torques at tEnd= 0.7680031232065901