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