[PYTHON] Spring damper with PyODE
I tried to implement a spring damper system with PyODE. So I checked if it works according to the physical properties.
1 Overview
Sites that I referred to without permission → https://w.atwiki.jp/bambooflow/pages/242.html
There are no springs or dampers in ODE. Therefore, the spring constant and damping factor are replaced with the values of ERP and CFM, which are the mutual constraint conditions for objects. (It seems to be written in the ODE manual.) In this article, we will connect two objects with slider joints and set ERP and CFM. ↓ This is the relevant partial code.
ODE spring damper setting method (using slider)
h = DT = 0.01 ###Let h be the time step length
kp = 20.0 #### Spring-rate [N/m]
kd = 8.944 #### Damping-rate
erp = h*kp / (h*kp + kd)
cfm = 1.0 / (h*kp + kd)
j = j01 = ode.SliderJoint( world )
j.attach( body0, body1 )
j.setAxis( (0,1,0) ) ####A slider that moves only in the Y-axis direction.
j.setParam(ode.ParamLoStop, 0.0) ####This is the standard on which springs and dampers work.
j.setParam(ode.ParamHiStop, 0.0) ####The standard on which springs and dampers work.
j.setParam(ode.ParamStopERP, erp)
j.setParam(ode.ParamStopCFM, cfm)
2 Code (whole description)
Previous article → "Sample of PyODE contact judgment and bound behavior" (https://qiita.com/emuai/items/652d36fb19b6f41dcd38) It was repaired based on the code of.
Code overview
・ 2 objects (no floor required)
-Place the first object `` body0``` at a height of 0 (*** Strictly 0 was a malfunction **, so it was invisible and shifted to a height of 1 mm). .. -Fixed body0 to the environment. -Place the second object ``` body1``` in the sky 1m above body0. -Connect body0 and` `body1 with a slider
・ Set the ERP / CFM value corresponding to the spring and damper on the slider.
-After the calculation is completed, a logical solution that matches the calculation conditions is generated and compared with the result for plotting.
↓ Code
slider_as_Spring-Damper
## pyODE example-1: with MPL-plot
## Appended ``Spring and dashpod'' to observe ocillation.
MILLI = 0.001
import ode
# Create a world object
world = ode.World()
DT = 0.001# 0.001 #0.04
G = 9.81
world.setGravity( (0,-G,0) )
R = 0.0001 #10.0 *MILLI
mass = 1.0
KP = 20.0 #### Spring-rate [N/m]
KD = 8.944 * 0.01 #0.25 #### Damping-rate
def get_body( pos, vel=(0.,0.,0.) ):
# Create a body inside the world
body = ode.Body(world)
M = ode.Mass()
#rho = 2700.0 ## AL??
m = mass
r = R
M.setSphereTotal( m, r )
M.mass = 1.0
body.setMass(M)
body.setPosition( pos )
#body.addForce( (0,200,0) )
body.setLinearVel( vel )
return body
body0 = get_body( (0.,0.001,0.) )
body0.setGravityMode( False )
body1 = get_body( (0.,1.,0.) )
#body1.setGravityMode( False )
ERP_GLOBAL = 1.0 #0.8 #1.0
CFM_GLOBAL = 0.0
COLLISION = not True
BOUNCE = 0.5
JOINT = True
if COLLISION or JOINT:
# Create a space object
space = ode.Space()
if COLLISION:
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane( space, (0,1,0), 0.1 )
geom0 = ode.GeomSphere( space, radius=R )
geom0.setBody( body0 )
if JOINT:
geom1 = ode.GeomSphere( space, radius=R )
geom1.setBody( body1 )
j = fix0 = ode.FixedJoint(world)
j.attach( body0, ode.environment )
j.setFixed()
j = j01 = ode.SliderJoint( world )
j.attach( body0, body1 )
j.setAxis( (0,1,0) )
h = step_size = DT# 0.04
kp = KP #20.0 #### Spring-rate [N/m]
kd = KD #8.944 * 0.01 #0.25 #0.0 #4.45 #8.9 #### Damping-rate
Cc = 2.0 * (mass*kp)**0.5 #### 8.944
zeta = kd / Cc
omega0 = (kp / mass )**0.5
erp = h*kp / (h*kp + kd)
cfm = 1.0 / (h*kp + kd)
j.setParam(ode.ParamLoStop, 0.0)
j.setParam(ode.ParamHiStop, 0.0)
j.setParam(ode.ParamStopERP, erp)
j.setParam(ode.ParamStopCFM, cfm)
world.setERP( ERP_GLOBAL ) #### ERP : Error Reduction Parameter
world.setCFM( CFM_GLOBAL ) #### CFM : Constraint Force Mixing
# A joint group for the contact joints that are generated whenever
# two bodies collide
contactgroup = ode.JointGroup()
# Collision callback
def near_callback(args, geom1, geom2): #### Unnecessary for Spring-Damper
""" Callback function for the collide() method.
This function checks if the given geoms do collide and
creates contact joints if they do.
"""
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
(world, contactgroup) = args
for c in contacts:
c.setBounce( BOUNCE )
c.setMu(5000)
j = ode.ContactJoint( world, contactgroup, c )
j.attach( geom1.getBody(), geom2.getBody() )
## Proceed the simulation...
total_time = 0.0
dt = DT #0.04 #0.04
import numpy as np
nt = 10000
txyzuvw = np.zeros( (7,nt+1) )
tn=0
END_TIME = 5.0
while total_time <= END_TIME:
body = body1
x,y,z = body.getPosition()
u,v,w = body.getLinearVel()
print( "%1.2fsec: pos=(%6.3f, %6.3f, %6.3f) vel=(%6.3f, %6.3f, %6.3f)" % (total_time, x, y, z, u,v,w) )
if tn <= nt:
txyzuvw[0][tn]=total_time
txyzuvw[1][tn]=x
txyzuvw[2][tn]=y
txyzuvw[3][tn]=z
txyzuvw[4][tn]=u
txyzuvw[5][tn]=v
txyzuvw[6][tn]=w
if COLLISION:
# Detect collisions and create contact joints
space.collide( (world,contactgroup), near_callback )
world.step(dt)
total_time+=dt
if COLLISION:
# Remove all contact joints
contactgroup.empty()
tn += 1
end_tn = tn
######## MPL-Plot
import matplotlib.pyplot as plt
PLOT_THEORY = True
if PLOT_THEORY:
import math
ys_zeta00 = np.zeros( end_tn )
ys_zeta05 = np.zeros( end_tn )
ys_zeta10 = np.zeros( end_tn )
ys_zeta15 = np.zeros( end_tn )
ys_zeta = np.zeros( end_tn )
A = (mass * G / kp)
y0 = 1.0-A
for tn in range( end_tn ):
t = txyzuvw[0][tn]
ot = omega0 *t
s = sigma = 0.0
#z1 = abs( z*z -1.0 )**0.5
y_zeta_00 = y0 +A *math.cos( ot )
z = 0.5
z1 = abs( z*z -1.0 )**0.5
z2= (s + z) / z1
A_ = A *( 1.0 + ( z2 )**2.0 )**0.5
alpha = math.atan( - z2 )
y_zeta_05 = y0 +A_ *math.exp( -z *ot) * math.cos( ot*z1 + alpha)
y_zeta_10 = y0 +A *math.exp( -ot ) *( (s+1.0) *ot +1 )
z = 1.5
z1 = abs( z*z -1.0 )**0.5
y_zeta_15 = y0 +A * math.exp( - z * ot ) * ( math.cosh( ot*z1 ) +( s+z) / z1 *math.sinh( ot *z1 ) )
'''
ys_zeta00[tn] = y_zeta_00
ys_zeta05[tn] = y_zeta_05
ys_zeta10[tn] = y_zeta_10
ys_zeta15[tn] = y_zeta_15
'''
z = zeta
z1 = abs( z*z -1.0 )**0.5
z2= (s + z) / z1
if z < 1.0:
A_ = A *( 1.0 + ( z2 )**2.0 )**0.5
alpha = math.atan( - z2 )
y_zeta = y0 +A_ *math.exp( -z *ot) * math.cos( ot*z1 + alpha)
if z == 1.0:
y_zeta = y_zeta10
elif z > 1.0:
y_zeta = y0 +A *math.exp( - z * ot ) *( math.cosh( ot*z1 ) +( s + z ) / z1 *math.sinh( ot *z1 ) )
ys_zeta[tn] = y_zeta
'''
plt.plot( txyzuvw[0][0:end_tn], ys_zeta00[0:end_tn], label=r'$\zeta=0$')
plt.plot( txyzuvw[0][0:end_tn], ys_zeta05[0:end_tn], label=r'$\zeta=0.5$')
plt.plot( txyzuvw[0][0:end_tn], ys_zeta10[0:end_tn], label=r'$\zeta=1$')
plt.plot( txyzuvw[0][0:end_tn], ys_zeta15[0:end_tn], label=r'$\zeta=1.5$')
'''
plt.plot( txyzuvw[0][0:end_tn], ys_zeta[0:end_tn], label=r'theory $\zeta=%g$'%(zeta), lw=5.0 )
plt.plot( txyzuvw[0][0:end_tn], txyzuvw[2][0:end_tn], label='Vertical position')
#plt.plot( txyzuvw[0][0:end_tn], txyzuvw[5][0:end_tn], label='Vertical velocity')
plt.xlabel('time [s]')
#plt.ylabel('Vertical position [m]')
plt.ylabel('Position [m] | Velocity [m/s]')
plt.legend()
plt.title( r'$k_{\mathrm{p} }=%g, k_{\mathrm{d}}=%g, C_{\mathrm{c}}=%g, \zeta=%g, \omega_{0}=%g$'%(kp,kd, Cc, zeta, omega0))
xmin = np.min( txyzuvw[0] )
xmax = np.max( txyzuvw[0] )
plt.hlines( [0], xmin, xmax, "blue", linestyles='dashed') # hlines
plt.tight_layout()
#savepath = './y_ERP%g_CFM%g_BR%g.png'%(ERP_GLOBAL, CFM_GLOBAL, BOUNCE)
savepath = './y_DT%g_kp%g_kd%g_zeta%g.png'%(DT, kp,kd, zeta )
plt.savefig( savepath )
print('An image-file exported : [%s]'%savepath )
#plt.show()
plt.pause(1.0)
The main purpose is to compare with the logical solution. I've commented it out, but now you can see plots with zeta = 0.0, 0.5, 1.0, 1.5.
3 Results
I plotted the height of the moving object at 1m above the sky ( Y = 1).
Since the mass is 1 kg and the spring constant is 20 [kg / m] under gravity, it should vibrate around Y = 0.5 m.
When I shook the attenuation rate `KD```, it sometimes deviated from the logical solution depending on the time step length `` DT```.
Here, I will show you two calculation results.
↓ First, it is the result of the time step length `` `DT = 0.01```.
DT = 0.01
At DT = 0.01, it does not match the logical solution (thick line). The calculation is too rough.
So reduce the DT by a factor of 10.
DT = 0.001
It has the same physical properties
Thus, `` `DT = 0.001``` was normal.
4 Summary
It should be noted that it does not move according to the physical properties depending on the time. ・ By the way, it seems that the time step is rougher under the condition that the viscosity is much stronger than the above example and the vibration does not appear and converges.
ODE's obscure behavior
Where I trapped this time:
-Fixed objects (" body0``` "in the code) do not seem to rest as set unless they are placed higher than `0.0``` (?)
What is it? In any case, if you have to do something like "Plot and check fixed objects", you may get stuck in a pot.