PyODEでスプリングダンパ系を実装してみました。
物性どおり動くか確認しました。
１ 概要
勝手ながら参考したサイト → https://w.atwiki.jp/bambooflow/pages/242.html
ODEにはスプリングやダンパそのものは無い。
そこで、バネ定数や減衰率をオブジェクト相互拘束条件であるERPとCFMの値に置き換えて与えます。
（ODEのマニュアルに書かれているそうです。）
この記事ではスライダ・ジョイントで２個のオブジェクトをつなぎ、ERP、CFMを設定します。
2023/10/02追記
（曖昧な理解ですが補足）
- LoStopとHiStopはスライダの可動範囲の指定です。
- axisをセットしたときの位置関係がポジション値0.0に相当
- 正負の向きはattach()に渡す順序とボディの向きに依存
↓ この例だと距離が離れる向きが正となるはず。
b0=ode.Body(world)
b0.setPosition( (0,0,0) )
b1=ode.Body(world)
b1.setPosition( (1,0,0) )
s01=ode.SliderJoint(world)
s01.attach(b1,b0)
s01.setAxis( XAXIS ) ## position value is set to 0
- スライダで拘束されているボディの方向が180度ひっくり返ってしまうときがある。このときボディ間距離とポジション値の関係も正負反転してしまう。
- LoStop,HiStop = 0.0,0.0 の設定はスライダとしての可動は無しということ。
- その「可動無し」の状態で、ERPとCFMの値を与えて拘束を緩めてやることで、スライダ軸の方向にバネのように動く。
（ERP=CFM=0.0のときには完全に拘束され、剛連結となる） - ※ちなみにスライダー可動領域にはParamERPの設定が無い（効かない）ようなのでここをバネにするには外力必要
↓ 当該部分コードです。
ODEのスプリング・ダンパ設定法（スライダ利用）
h = DT = 0.01 ### 時間刻み長をhとします
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) ) #### Y軸方向にのみ動くスライダってこと。
j.setParam(ode.ParamLoStop, 0.0) #### Lo=0,Hi=0 でスライダ可動域をゼロに。
j.setParam(ode.ParamHiStop, 0.0)
j.setParam(ode.ParamStopERP, erp) #### erpとcfmを大きくするほど拘束が緩む。
j.setParam(ode.ParamStopCFM, cfm)
2 コード(丸ごと記載)
先の記事→「PyODE 接触判定とバウンド挙動のサンプル」(https://qiita.com/emuai/items/652d36fb19b6f41dcd38)
のコードを元に改修しました。
コードの概要
・オブジェクトは2個 （床は不要）
・1個目のオブジェクトbody0を高さ0に設置（※厳密に 0 だと動作不良だったため、見えないていどにずらし 1mm の高さとした。）に置く。
・body0 を環境に対し固定。
・2個めのオブジェクト body1 を body0 の上空 1m に置く。
・body0とbody1をスライダ接続
・スライダにバネとダンパに相当するERP/CFM値を設定
・計算終了後には、計算条件に合わせた論理解を生成し、結果と比較プロットする。
↓ コード
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)
論理解と比較することを主眼にしています。
コメントアウトしていますが、zeta＝0.0、0.5、1.0、1.5 のプロットを一覧できるようになっています。
3 結果
上空1m（Y=1）にある動く方のオブジェクトの高さをプロットしました。
重力下で質量1kg、バネ係数20[kg/m]なので Y=0.5m のあたりを中心に振動するはずです。
減衰率 KD を振ってみたところ、時間刻み長 DT によっては論理解から逸脱するときがありました。
ここでは2つの計算結果お見せします。
↓ まず、時間刻み長DT = 0.01の結果です。
DT = 0.01
DT=0.01では、論理解（太線）と合いません。計算が荒すぎるということです。
そこで DT を10分の1します。
DT = 0.001
おなじ物性ですが
このように DT = 0.001 では正常でした。
4 まとめ
このように時間刻み次第で物性通りに動かないことに注意する必要があります。
・ちなみに上の例より粘性がずっと強く、振動が現れずに収束するような条件のほうが、時間刻みは荒めで済むようでした。
ODEのよくわからない挙動
今回トラップったところ：
・固定オブジェクト(コードの「body0」)は、 0.0 より高い位置に設置しないと、設定どおりに静止しないようだ（？）
なんでしょうね？
なににせよ「固定したオブジェクトもプロットして確認」とかしとかないと、どつぼにはまるかも。