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