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