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
63[nFoot,bFoot]=AddRigidBody(mainSys = mbs,
64 inertia = inertiaFoot,
65 nodeType = exu.NodeType.RotationEulerParameters,
66 position = [0,0,rFoot+z0],
67 gravity = g,
68 graphicsDataList = [graphicsFoot])
69
70[nLeg,bLeg]=AddRigidBody(mainSys = mbs,
71 inertia = inertiaLeg,
72 nodeType = exu.NodeType.RotationEulerParameters,
73 position = [0,0,0.5*lLeg+rFoot+z0],
74 gravity = g,
75 graphicsDataList = [graphicsLeg])
76
77[nFemoral,bFemoral]=AddRigidBody(mainSys = mbs,
78 inertia = inertiaFemoral,
79 nodeType = exu.NodeType.RotationEulerParameters,
80 position = [0,0,0.5*lFemoral + lLeg+rFoot+z0],
81 gravity = g,
82 graphicsDataList = [graphicsFemoral])
83
84[nBody,bBody]=AddRigidBody(mainSys = mbs,
85 inertia = inertiaBody,
86 nodeType = exu.NodeType.RotationEulerParameters,
87 position = [0,0,0.5*dBody + lFemoral + lLeg+rFoot+z0],
88 gravity = g,
89 graphicsDataList = [graphicsBody])
90
91
92
93#%%++++++++++++++++++++++++++++++++++++++++++++++++
94#ground body and marker
95gGround = graphics.CheckerBoard(point=[0,0,0], size=4)
96oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
97markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
98
99#markers for rigid bodies:
100markerFoot = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bFoot, localPosition=[0,0,0]))
101
102markerLegA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bLeg, localPosition=[0,0,-0.5*lLeg]))
103markerLegB = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bLeg, localPosition=[0,0, 0.5*lLeg]))
104
105markerFemoralA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bFemoral, localPosition=[0,0,-0.5*lFemoral]))
106markerFemoralB = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bFemoral, localPosition=[0,0, 0.5*lFemoral]))
107
108markerBodyA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bBody, localPosition=[0,0,-0.5*dBody]))
109
110#%%++++++++++++++++++++++++++++++++++++++++++++++++
111#add 'rolling disc' contact for foot:
112cStiffness = 5e4 #spring stiffness: 50N==>F/k = u = 0.001m (penetration)
113cDamping = cStiffness*0.05 #think on a one-mass spring damper
114nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
115oRolling=mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[markerGround, markerFoot],
116 nodeNumber = nGeneric,
117 discRadius=rFoot,
118 dryFriction=[0.8,0.8],
119 dryFrictionProportionalZone=1e-2,
120 rollingFrictionViscous=0.2,
121 contactStiffness=cStiffness,
122 contactDamping=cDamping,
123 #activeConnector = False, #set to false to deactivated
124 visualization=VObjectConnectorRollingDiscPenalty(discWidth=dFoot, color=graphics.color.blue)))
125
126#%%++++++++++++++++++++++++++++++++++++++++++++++++
127#add joints to legs:
128aR = 0.02
129aL = 0.1
130oJointLeg = mbs.AddObject(GenericJoint(markerNumbers=[markerFoot, markerLegA],
131 constrainedAxes=[1,1,1,1,1,1],
132 visualization=VGenericJoint(axesRadius=aR, axesLength=aL)))
133oJointFemoral = mbs.AddObject(GenericJoint(markerNumbers=[markerLegB, markerFemoralA],
134 constrainedAxes=[1,1,1,0,1,1],
135 visualization=VGenericJoint(axesRadius=aR, axesLength=aL)))
136oJointBody = mbs.AddObject(GenericJoint(markerNumbers=[markerFemoralB, markerBodyA],
137 constrainedAxes=[1,1,1,1*0,1,1],
138 visualization=VGenericJoint(axesRadius=aR, axesLength=aL)))
139
140#stabilize body2:
141# markerGroundBody = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,lFemoral + lLeg+rFoot+z0]))
142# oJointBody2 = mbs.AddObject(GenericJoint(markerNumbers=[markerGroundBody, markerBodyA],
143# constrainedAxes=[1,1,1,1,1,1],
144# visualization=VGenericJoint(axesRadius=aR, axesLength=aL)))
145
146def SmoothStepDerivative(x, x0, x1, value0, value1):
147 loadValue = 0
148
149 if x > x0 and x < x1:
150 dx = x1-x0
151 loadValue = (value1-value0) * 0.5*(pi/dx*sin((x-x0)/dx*pi))
152 return loadValue
153
154#%%++++++++++++++++++++++++++++++++++++++++++++++++
155#add sensors and torques for control
156sJointFemoral = mbs.AddSensor(SensorObject(objectNumber=oJointFemoral, fileName='solution/anglesJointFemoral',
157 outputVariableType=exu.OutputVariableType.Rotation))
158sJointFemoralVel = mbs.AddSensor(SensorObject(objectNumber=oJointFemoral, fileName='solution/anglesJointFemoralVel',
159 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
160sJointBody = mbs.AddSensor(SensorObject(objectNumber=oJointBody, fileName='solution/anglesJointBody',
161 outputVariableType=exu.OutputVariableType.Rotation))
162sJointBodyVel = mbs.AddSensor(SensorObject(objectNumber=oJointBody, fileName='solution/anglesJointBodyVel',
163 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
164
165pControl = 50*2
166dControl = 5
167t0Leg = 1.5
168t1Leg = 0.5+t0Leg
169t0Leg2 = 2
170t1Leg2 = 0.15+t0Leg2
171
172ang = 30
173phiEnd = 2*ang*pi/180
174phiEnd2 = -2*ang*pi/180
175
176f=1
177dt0=0.05*f
178dt1=0.2*f+dt0
179dt2=0.1*f+dt1
180
181def phiLeg(t):
182 return (SmoothStep(t, t0Leg, t1Leg, 0, phiEnd) +
183 SmoothStep(t, t0Leg2, t1Leg2, 0, phiEnd2) +
184 SmoothStep(t, t1Leg2+dt0, t1Leg2+dt1, 0, phiEnd) +
185 SmoothStep(t, t1Leg2+dt1, t1Leg2+dt2, 0, phiEnd2) +
186 SmoothStep(t, t1Leg2+dt0+dt2, t1Leg2+dt1+dt2, 0, phiEnd) +
187 SmoothStep(t, t1Leg2+dt1+dt2, t1Leg2+dt2+dt2, 0, phiEnd2)
188 )
189def phiLeg_t(t):
190 return (SmoothStepDerivative(t, t0Leg, t1Leg, 0, phiEnd) +
191 SmoothStepDerivative(t, t0Leg2, t1Leg2, 0, phiEnd2) +
192 SmoothStepDerivative(t, t1Leg2+dt0, t1Leg2+dt1, 0, phiEnd) +
193 SmoothStepDerivative(t, t1Leg2+dt1, t1Leg2+dt2, 0, phiEnd2) +
194 SmoothStepDerivative(t, t1Leg2+dt0+dt2, t1Leg2+dt1+dt2, 0, phiEnd) +
195 SmoothStepDerivative(t, t1Leg2+dt1+dt2, t1Leg2+dt2+dt2, 0, phiEnd2)
196 )
197
198def LegTorqueControl(mbs, t, loadVector):
199 s = loadVector[0] #sign
200 phiDesired = phiLeg(t)
201 phi_tDesired = phiLeg_t(t)
202 phi = mbs.GetSensorValues(sJointFemoral)[0]
203 phi_t = mbs.GetSensorValues(sJointFemoralVel)[0]
204 #print("leg phi=",phi*180/pi, "phiD=", phiDesired*180/pi)
205 T = (phiDesired-phi)*pControl + (phi_tDesired-phi_t)*dControl
206 return [s*T,0,0]
207
208pControlFemoral = 50*2
209dControlFemoral = 5
210t0Femoral = 0
211t1Femoral = 0.5+t0Femoral
212phiEndFemoral = 9.5*pi/180
213phiEndFemoral2 = -ang*pi/180-phiEndFemoral
214
215def FemoralTorqueControl(mbs, t, loadVector):
216 s = loadVector[0] #sign
217 phiDesired = (SmoothStep(t, t0Femoral, t1Femoral, 0, phiEndFemoral)
218 + SmoothStep(t, 1.5, 2, 0, -2*phiEndFemoral)
219 - 0.5*phiLeg(t))
220
221 phi_tDesired = (SmoothStepDerivative(t, t0Femoral, t1Femoral, 0, phiEndFemoral)
222 + SmoothStepDerivative(t, 1.5, 2, 0, -2*phiEndFemoral)
223 - 0.5*phiLeg_t(t))
224
225 phi = mbs.GetSensorValues(sJointBody)[0]
226 phi_t = mbs.GetSensorValues(sJointBodyVel)[0]
227 #print("phi=",phi*180/pi, "phiD=", phiDesired*180/pi)
228 T = (phiDesired-phi)*pControlFemoral + (phi_tDesired-phi_t)*dControlFemoral
229 return [s*T,0,0]
230
231
232loadLegB = mbs.AddLoad(LoadTorqueVector(markerNumber=markerLegB, loadVector=[-1,0,0], #negative sign
233 bodyFixed=True, loadVectorUserFunction=LegTorqueControl))
234loadFemoralA = mbs.AddLoad(LoadTorqueVector(markerNumber=markerFemoralA, loadVector=[1,0,0], #positive sign
235 bodyFixed=True, loadVectorUserFunction=LegTorqueControl))
236
237loadFemoralB = mbs.AddLoad(LoadTorqueVector(markerNumber=markerFemoralB, loadVector=[-1,0,0], #positive sign
238 bodyFixed=True, loadVectorUserFunction=FemoralTorqueControl))
239loadBody = mbs.AddLoad(LoadTorqueVector(markerNumber=markerBodyA, loadVector=[1,0,0], #negative sign
240 bodyFixed=True, loadVectorUserFunction=FemoralTorqueControl))
241
242sLeg = mbs.AddSensor(SensorLoad(loadNumber=loadLegB, fileName='solution/torqueLeg.txt'))
243sFemoral = mbs.AddSensor(SensorLoad(loadNumber=loadFemoralB, fileName='solution/torqueFemoral.txt'))
244
245#%%++++++++++++++++++++++++++++++++++++++++++++++++
246#simulate:
247mbs.Assemble()
248
249simulationSettings = exu.SimulationSettings() #takes currently set values or default values
250
251tEnd = 2.8
252h=0.0002 #use small step size to detext contact switching
253
254simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
255simulationSettings.timeIntegration.endTime = tEnd
256simulationSettings.solutionSettings.writeSolutionToFile= False
257simulationSettings.solutionSettings.sensorsWritePeriod = 0.0005
258simulationSettings.timeIntegration.verboseMode = 1
259
260simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6
261simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
262
263
264SC.visualizationSettings.nodes.show = True
265SC.visualizationSettings.nodes.drawNodesAsPoint = False
266SC.visualizationSettings.nodes.showBasis = True
267SC.visualizationSettings.nodes.basisSize = 0.015
268
269if False: #record animation frames:
270 SC.visualizationSettings.exportImages.saveImageFileName = "animation/frame"
271 SC.visualizationSettings.window.renderWindowSize=[1980,1080]
272 SC.visualizationSettings.openGL.multiSampling = 4
273 simulationSettings.solutionSettings.recordImagesInterval = 0.01
274
275SC.visualizationSettings.general.autoFitScene = False #use loaded render state
276useGraphics = True
277if useGraphics:
278 exu.StartRenderer()
279 if 'renderState' in exu.sys:
280 SC.SetRenderState(exu.sys[ 'renderState' ])
281 mbs.WaitForUserToContinue()
282mbs.SolveDynamic(simulationSettings)
283
284
285if useGraphics:
286 SC.WaitForRenderEngineStopFlag()
287 exu.StopRenderer() #safely close rendering window!
288
289 ##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
290 #plot results
291
292 mbs.PlotSensor(sensorNumbers=[sLeg,sFemoral], components=[0,0])
293
294
295 if False:
296 import matplotlib.pyplot as plt
297 import matplotlib.ticker as ticker
298
299 data = np.loadtxt('solution/rollingDiscPos.txt', comments='#', delimiter=',')
300 plt.plot(data[:,0], data[:,1], 'r-',label='coin pos x')
301 plt.plot(data[:,0], data[:,2], 'g-',label='coin pos y')
302 plt.plot(data[:,0], data[:,3], 'b-',label='coin pos z')
303
304 ax=plt.gca() # get current axes
305 ax.grid(True, 'major', 'both')
306 ax.xaxis.set_major_locator(ticker.MaxNLocator(10)) #use maximum of 8 ticks on y-axis
307 ax.yaxis.set_major_locator(ticker.MaxNLocator(10)) #use maximum of 8 ticks on y-axis
308 plt.tight_layout()
309 plt.legend()
310 plt.show()