PyODEでバネダンパ


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の設定が無い（効かない）ようなのでここをバネにするには外力必要

↓　当該部分コードです。

```python: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値を設定
・計算終了後には、計算条件に合わせた論理解を生成し、結果と比較プロットする。

↓　コード

```python: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
![y_DT0.01_kp20_kd0.08944_zeta0.0099997.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/469875/404b272b-788f-9486-f696-d2eed595bb8d.png)
DT=0.01では、論理解（太線）と合いません。計算が荒すぎるということです。

そこで DT を10分の1します。
#### DT = 0.001
![y_DT0.001_kp20_kd0.08944_zeta0.0099997.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/469875/74fe209c-b1f4-e9e9-29bc-06d8aabce271.png)
おなじ物性ですが
このように ```DT = 0.001``` では正常でした。

## 4 まとめ
このように時間刻み次第で物性通りに動かないことに注意する必要があります。
　・ちなみに上の例より粘性がずっと強く、振動が現れずに収束するような条件のほうが、時間刻みは荒めで済むようでした。
 
## ODEのよくわからない挙動
今回トラップったところ：
・固定オブジェクト(コードの「```body0```」)は、 ```0.0``` より高い位置に設置しないと、設定どおりに静止しないようだ（？）

なんでしょうね？
なににせよ「固定したオブジェクトもプロットして確認」とかしとかないと、どつぼにはまるかも。