leggedRobot.py
You can view and download this file on Github: leggedRobot.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: legged robot example with contact using a rolling disc
5#
6# Author: Johannes Gerstmayr
7# Date: 2021-05-19
8#
9# 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.
10#
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13
14import exudyn as exu
15#a local run must not open a plot window when windows are suppressed (#2477)
16if exu.special.userInterface.suppressPlots: import matplotlib; matplotlib.use('Agg')
17from exudyn.itemInterface import *
18from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
19import exudyn.graphics as graphics
20
21from math import sin, cos, pi
22import numpy as np
23
24SC = exu.SystemContainer()
25mbs = SC.AddSystem()
26
27phi0 = 0
28g = [0,0,-9.81] #gravity in m/s^2
29
30# initialRotation = RotationMatrixY(phi0)
31# omega0 = [40,0,0*1800/180*np.pi] #initial angular velocity around z-axis
32# v0 = Skew(omega0) @ initialRotation @ [0,0,r] #initial angular velocity of center point
33
34#mass assumptions
35rFoot = 0.1
36lLeg = 0.4
37lFemoral = 0.4
38dFoot = 0.05
39dLeg = 0.04
40dFemoral = 0.05
41dBody = 0.2
42
43massFoot = 0.1
44massLeg = 0.3
45massFemoral = 0.5
46massBody = 4
47
48#p0 = [0,0,rFoot] #origin of disc center point at reference, such that initial contact point is at [0,0,0]
49
50#%%++++++++++++++++++++++++++++++++++++++++++++++++
51#inertia assumptions:
52inertiaFoot = InertiaCylinder(density=massFoot/(dFoot*rFoot**2*pi), length=dFoot, outerRadius=rFoot, axis=0)
53inertiaLeg = InertiaCuboid(density=massLeg/(lLeg*dLeg**2), sideLengths=[dLeg, dLeg, lLeg])
54inertiaFemoral = InertiaCuboid(density=massFemoral/(lFemoral*dFemoral**2), sideLengths=[dFemoral, dFemoral, lFemoral])
55inertiaBody = InertiaCuboid(density=massBody/(dBody**3), sideLengths=[dBody,dBody,dBody])
56
57graphicsFoot = graphics.Brick(centerPoint=[0,0,0],size=[dFoot*1.1,0.7*rFoot,0.7*rFoot], color=graphics.color.lightred)
58graphicsLeg = graphics.Brick(centerPoint=[0,0,0],size=[dLeg, dLeg, lLeg], color=graphics.color.steelblue)
59graphicsFemoral = graphics.Brick(centerPoint=[0,0,0],size=[dFemoral, dFemoral, lFemoral], color=graphics.color.lightgrey)
60graphicsBody = graphics.Brick(centerPoint=[0,0,0],size=[dBody,dBody,dBody], color=graphics.color.green)
61
62z0 = 0*0.1 #initial offset
63#foot, lower leg, femoral
64dictFoot = mbs.CreateRigidBody(
65 inertia=inertiaFoot,
66 nodeType=exu.NodeType.RotationEulerParameters,
67 referencePosition=[0,0,rFoot+z0],
68 gravity=g,
69 graphicsDataList=[graphicsFoot],
70 returnDict=True)
71[nFoot, bFoot] = [dictFoot['nodeNumber'], dictFoot['bodyNumber']]
72
73dictLeg = mbs.CreateRigidBody(
74 inertia=inertiaLeg,
75 nodeType=exu.NodeType.RotationEulerParameters,
76 referencePosition=[0,0,0.5*lLeg+rFoot+z0],
77 gravity=g,
78 graphicsDataList=[graphicsLeg],
79 returnDict=True)
80[nLeg, bLeg] = [dictLeg['nodeNumber'], dictLeg['bodyNumber']]
81
82dictFemoral = mbs.CreateRigidBody(
83 inertia=inertiaFemoral,
84 nodeType=exu.NodeType.RotationEulerParameters,
85 referencePosition=[0,0,0.5*lFemoral + lLeg+rFoot+z0],
86 gravity=g,
87 graphicsDataList=[graphicsFemoral],
88 returnDict=True)
89[nFemoral, bFemoral] = [dictFemoral['nodeNumber'], dictFemoral['bodyNumber']]
90
91dictBody = mbs.CreateRigidBody(
92 inertia=inertiaBody,
93 nodeType=exu.NodeType.RotationEulerParameters,
94 referencePosition=[0,0,0.5*dBody + lFemoral + lLeg+rFoot+z0],
95 gravity=g,
96 graphicsDataList=[graphicsBody],
97 returnDict=True)
98[nBody, bBody] = [dictBody['nodeNumber'], dictBody['bodyNumber']]
99
100
101
102#%%++++++++++++++++++++++++++++++++++++++++++++++++
103#ground body and marker
104gGround = graphics.CheckerBoard(point=[0,0,0], size=4)
105oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
106markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
107
108#markers for rigid bodies:
109markerFoot = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bFoot, localPosition=[0,0,0]))
110
111markerLegA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bLeg, localPosition=[0,0,-0.5*lLeg]))
112markerLegB = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bLeg, localPosition=[0,0, 0.5*lLeg]))
113
114markerFemoralA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bFemoral, localPosition=[0,0,-0.5*lFemoral]))
115markerFemoralB = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bFemoral, localPosition=[0,0, 0.5*lFemoral]))
116
117markerBodyA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bBody, localPosition=[0,0,-0.5*dBody]))
118
119#%%++++++++++++++++++++++++++++++++++++++++++++++++
120#add 'rolling disc' contact for foot:
121cStiffness = 5e4 #spring stiffness: 50N==>F/k = u = 0.001m (penetration)
122cDamping = cStiffness*0.05 #think on a one-mass spring damper
123nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
124oRolling=mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[markerGround, markerFoot],
125 nodeNumber = nGeneric,
126 discRadius=rFoot,
127 dryFriction=[0.8,0.8],
128 dryFrictionProportionalZone=1e-2,
129 rollingViscousFriction=0.2,
130 contactStiffness=cStiffness,
131 contactDamping=cDamping,
132 #activeConnector = False, #set to false to deactivated
133 visualization=VObjectConnectorRollingDiscPenalty(discWidth=dFoot, color=graphics.color.blue)))
134
135#%%++++++++++++++++++++++++++++++++++++++++++++++++
136#add joints to legs:
137aR = 0.02
138aL = 0.1
139oJointLeg = mbs.AddObject(GenericJoint(markerNumbers=[markerFoot, markerLegA],
140 constrainedAxes=[1,1,1,1,1,1],
141 visualization=VGenericJoint(axesRadius=aR, axesLength=aL)))
142oJointFemoral = mbs.AddObject(GenericJoint(markerNumbers=[markerLegB, markerFemoralA],
143 constrainedAxes=[1,1,1,0,1,1],
144 visualization=VGenericJoint(axesRadius=aR, axesLength=aL)))
145oJointBody = mbs.AddObject(GenericJoint(markerNumbers=[markerFemoralB, markerBodyA],
146 constrainedAxes=[1,1,1,1*0,1,1],
147 visualization=VGenericJoint(axesRadius=aR, axesLength=aL)))
148
149#stabilize body2:
150# markerGroundBody = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,lFemoral + lLeg+rFoot+z0]))
151# oJointBody2 = mbs.AddObject(GenericJoint(markerNumbers=[markerGroundBody, markerBodyA],
152# constrainedAxes=[1,1,1,1,1,1],
153# visualization=VGenericJoint(axesRadius=aR, axesLength=aL)))
154
155def SmoothStepDerivative(x, x0, x1, value0, value1):
156 loadValue = 0
157
158 if x > x0 and x < x1:
159 dx = x1-x0
160 loadValue = (value1-value0) * 0.5*(pi/dx*sin((x-x0)/dx*pi))
161 return loadValue
162
163#%%++++++++++++++++++++++++++++++++++++++++++++++++
164#add sensors and torques for control
165sJointFemoral = mbs.AddSensor(SensorObject(objectNumber=oJointFemoral, fileName='solution/anglesJointFemoral',
166 outputVariableType=exu.OutputVariableType.Rotation))
167sJointFemoralVel = mbs.AddSensor(SensorObject(objectNumber=oJointFemoral, fileName='solution/anglesJointFemoralVel',
168 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
169sJointBody = mbs.AddSensor(SensorObject(objectNumber=oJointBody, fileName='solution/anglesJointBody',
170 outputVariableType=exu.OutputVariableType.Rotation))
171sJointBodyVel = mbs.AddSensor(SensorObject(objectNumber=oJointBody, fileName='solution/anglesJointBodyVel',
172 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
173
174pControl = 50*2
175dControl = 5
176t0Leg = 1.5
177t1Leg = 0.5+t0Leg
178t0Leg2 = 2
179t1Leg2 = 0.15+t0Leg2
180
181ang = 30
182phiEnd = 2*ang*pi/180
183phiEnd2 = -2*ang*pi/180
184
185f=1
186dt0=0.05*f
187dt1=0.2*f+dt0
188dt2=0.1*f+dt1
189
190def phiLeg(t):
191 return (SmoothStep(t, t0Leg, t1Leg, 0, phiEnd) +
192 SmoothStep(t, t0Leg2, t1Leg2, 0, phiEnd2) +
193 SmoothStep(t, t1Leg2+dt0, t1Leg2+dt1, 0, phiEnd) +
194 SmoothStep(t, t1Leg2+dt1, t1Leg2+dt2, 0, phiEnd2) +
195 SmoothStep(t, t1Leg2+dt0+dt2, t1Leg2+dt1+dt2, 0, phiEnd) +
196 SmoothStep(t, t1Leg2+dt1+dt2, t1Leg2+dt2+dt2, 0, phiEnd2)
197 )
198def phiLeg_t(t):
199 return (SmoothStepDerivative(t, t0Leg, t1Leg, 0, phiEnd) +
200 SmoothStepDerivative(t, t0Leg2, t1Leg2, 0, phiEnd2) +
201 SmoothStepDerivative(t, t1Leg2+dt0, t1Leg2+dt1, 0, phiEnd) +
202 SmoothStepDerivative(t, t1Leg2+dt1, t1Leg2+dt2, 0, phiEnd2) +
203 SmoothStepDerivative(t, t1Leg2+dt0+dt2, t1Leg2+dt1+dt2, 0, phiEnd) +
204 SmoothStepDerivative(t, t1Leg2+dt1+dt2, t1Leg2+dt2+dt2, 0, phiEnd2)
205 )
206
207def LegTorqueControl(mbs, t, loadVector):
208 s = loadVector[0] #sign
209 phiDesired = phiLeg(t)
210 phi_tDesired = phiLeg_t(t)
211 phi = mbs.GetSensorValues(sJointFemoral)[0]
212 phi_t = mbs.GetSensorValues(sJointFemoralVel)[0]
213 #print("leg phi=",phi*180/pi, "phiD=", phiDesired*180/pi)
214 T = (phiDesired-phi)*pControl + (phi_tDesired-phi_t)*dControl
215 return [s*T,0,0]
216
217pControlFemoral = 50*2
218dControlFemoral = 5
219t0Femoral = 0
220t1Femoral = 0.5+t0Femoral
221phiEndFemoral = 9.5*pi/180
222phiEndFemoral2 = -ang*pi/180-phiEndFemoral
223
224def FemoralTorqueControl(mbs, t, loadVector):
225 s = loadVector[0] #sign
226 phiDesired = (SmoothStep(t, t0Femoral, t1Femoral, 0, phiEndFemoral)
227 + SmoothStep(t, 1.5, 2, 0, -2*phiEndFemoral)
228 - 0.5*phiLeg(t))
229
230 phi_tDesired = (SmoothStepDerivative(t, t0Femoral, t1Femoral, 0, phiEndFemoral)
231 + SmoothStepDerivative(t, 1.5, 2, 0, -2*phiEndFemoral)
232 - 0.5*phiLeg_t(t))
233
234 phi = mbs.GetSensorValues(sJointBody)[0]
235 phi_t = mbs.GetSensorValues(sJointBodyVel)[0]
236 #print("phi=",phi*180/pi, "phiD=", phiDesired*180/pi)
237 T = (phiDesired-phi)*pControlFemoral + (phi_tDesired-phi_t)*dControlFemoral
238 return [s*T,0,0]
239
240
241loadLegB = mbs.AddLoad(LoadTorqueVector(markerNumber=markerLegB, loadVector=[-1,0,0], #negative sign
242 bodyFixed=True, loadVectorUserFunction=LegTorqueControl))
243loadFemoralA = mbs.AddLoad(LoadTorqueVector(markerNumber=markerFemoralA, loadVector=[1,0,0], #positive sign
244 bodyFixed=True, loadVectorUserFunction=LegTorqueControl))
245
246loadFemoralB = mbs.AddLoad(LoadTorqueVector(markerNumber=markerFemoralB, loadVector=[-1,0,0], #positive sign
247 bodyFixed=True, loadVectorUserFunction=FemoralTorqueControl))
248loadBody = mbs.AddLoad(LoadTorqueVector(markerNumber=markerBodyA, loadVector=[1,0,0], #negative sign
249 bodyFixed=True, loadVectorUserFunction=FemoralTorqueControl))
250
251sLeg = mbs.AddSensor(SensorLoad(loadNumber=loadLegB, fileName='solution/torqueLeg.txt'))
252sFemoral = mbs.AddSensor(SensorLoad(loadNumber=loadFemoralB, fileName='solution/torqueFemoral.txt'))
253
254#%%++++++++++++++++++++++++++++++++++++++++++++++++
255#simulate:
256mbs.Assemble()
257
258simulationSettings = exu.SimulationSettings() #takes currently set values or default values
259
260tEnd = 2.8
261h=0.0002 #use small step size to detext contact switching
262
263simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
264simulationSettings.timeIntegration.endTime = tEnd
265simulationSettings.solution.file.write= False
266simulationSettings.solution.sensors.writePeriod = 0.0005
267simulationSettings.timeIntegration.verboseMode = 1
268
269simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6
270simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
271
272
273SC.visualizationSettings.nodes.show = True
274SC.visualizationSettings.nodes.drawNodesAsPoint = False
275SC.visualizationSettings.nodes.showBasis = True
276SC.visualizationSettings.nodes.basisSize = 0.015
277
278if False: #record animation frames:
279 SC.visualizationSettings.exportImages.saveImageFileName = "animation/frame"
280 SC.visualizationSettings.view0.window.renderWindowSize=[1980,1080]
281 SC.visualizationSettings.openGL.multiSampling = 4
282 simulationSettings.solution.recordImagesInterval = 0.01
283
284SC.visualizationSettings.general.autoFitScene = False #use loaded render state
285useGraphics = True
286if useGraphics:
287 SC.renderer.Start()
288 SC.renderer.RestoreSavedState()
289 SC.renderer.DoIdleTasks()
290mbs.SolveDynamic(simulationSettings)
291
292
293if useGraphics:
294 SC.renderer.DoIdleTasks()
295 SC.renderer.Stop() #safely close rendering window!
296
297 ##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
298 #plot results
299
300 mbs.PlotSensor(sensorNumbers=[sLeg,sFemoral], components=[0,0])
301
302
303 if False:
304 import matplotlib.pyplot as plt
305 import matplotlib.ticker as ticker
306
307 data = np.loadtxt(OutputFilePath('solution/rollingDiscPos.txt'), comments='#', delimiter=',')
308 plt.plot(data[:,0], data[:,1], 'r-',label='coin pos x')
309 plt.plot(data[:,0], data[:,2], 'g-',label='coin pos y')
310 plt.plot(data[:,0], data[:,3], 'b-',label='coin pos z')
311
312 ax=plt.gca() # get current axes
313 ax.grid(True, 'major', 'both')
314 ax.xaxis.set_major_locator(ticker.MaxNLocator(10)) #use maximum of 8 ticks on y-axis
315 ax.yaxis.set_major_locator(ticker.MaxNLocator(10)) #use maximum of 8 ticks on y-axis
316 plt.tight_layout()
317 plt.legend()
318 plt.show()