PyODE スライダーを2重にしてみる

ことのなりゆき

先の記事 ``PyODEでバネダンパ'' にて
ODEのスライダ（ode.SliderJoint）によりバネ・ダンパを構成しました。

ただし、筆者は複合バネがやりたいのです。
異なる性質のバネやダンパを平衡接続とか必要だからスライダ1個ではあまり意味がない。
（それらをスライダで構成すべきことかどうかもまだわかりません。）

それで、試しに 2オブジェクト間のスライダを2重にできるか試してみました。

実施したこと

　・2個のオブジェクトの間にスライダ2個入れてみた
　・worldのCFM値調整
　・Pygame の可視化を追加

先に結論:

　・world の CFM の値の調整が必要だったが、スライド2重は構成可能。

CFMの調整が必須

※このERP / CFMはスライダのERP / CFMとはまた別なものです注意。

スライダ1個のときは

ERP_GLOBAL = 1.0
CFM_GLOBAL = 0.0
（中略）
world.setERP( ERP_GLOBAL )   ####    ERP : Error Reduction Parameter 
world.setCFM( CFM_GLOBAL )   ####    CFM : Constraint Force Mixing

というように ERP=1.0、CFM=0.0 としていて、それで動いてました。

とはいえ、この 1.0 と 0.0 というのは本来は無理のある値だとおもいます。
ジョイントが1個のみで干渉しあうものがないから動いたのでしょう。

そのままの設定でスライダを足して2個にし、実行したところ
スタートと同時にこんなメッセージが出て停まりました。

停まること自体は案の定といってもよいです。
けれども、このメッセージは不親切だと思う。読んでもなにが原因かまったくわかりません。

今は、ERP／CFM の調整が念頭にあったので　ひとまず

ERP_GLOBAL = 1.0
CFM_GLOBAL = 1.0e-6 #0.0

に変えました。動くようになりました。

今回の構成では ERP は 1.0 のままでも問題ありませんでした。

実行結果

Pygame で2D可視化しました。
PyODE 「Tutorial 2」のやり方に倣っています。
（余白がやたら多いのだけどそういう調整には時間かけないと思っている）

　↑　黒丸が固定オブジェクトでその上方に SliderJoint 2本を介して赤茶丸を繋げてある。
スライダの線は左右にオフセットつけて描画していますがODE上は重なっています。
（掲載コードはオフセットつけてない状態です）

赤茶丸の位置vs時刻プロットは前のケースと同様に出力してます。↓

青線：論理解、橙線：ODE計算、　縦軸ラベルにVelocityてあるのは消し忘れ

スライダ2重にするかわりに、バネ・ダンパの係数をそれぞれ元の2分の1にしています。
すると、前記事と同じ結果になるわけです。

コード（全部）

だいぶごちゃついてきましたが
今回もまるごと載せます。

Pygame の可視化を追加してあります。

####    tutorial_1_3.py
##  pyODE example-1: with MPL-plot
##  Appended: ``Spring and dashpod'' to observe ocillation.
##  Appended: Pygame 2d-visualization 

import pygame
from pygame.locals import *
#import ode

#DT = 0.05 #1
KEEP_FPS = not True #False
FPS = 10. #30.
RENDERING_INTERVAL = 100 #10
GIF = True

W = 640
H = 640
CX = 320
CY = 320
S = 200.0
def coord(xyz):
    (x,y,z) = xyz
    "Convert world coordinates to pixel coordinates."
    return int( CX +S*x ), int( CY -S*y)



from PIL import Image, ImageDraw

def storeImage( srfc, images ):
    if NI_MAX <= len(images):
        return
    s = srfc
    buf = s.get_buffer()
    im = Image.frombytes( "RGBA", s.get_size(), buf.raw )
    B,G,R,A = im.split()
    img = Image.merge("RGBA",(R,G,B,A))
    images.append(img)

def gif(images):
    print(' @ gif(), ')

    image0 = images[0]
    image_end = images[-1]
    for i in range( 5 ):
        images.insert(0,image0)
    for i in range( 5 ):
        images.append(image_end)

    savepath = 'tutorial_1.gif'
    images[0].save( savepath, save_all=True, append_images=images[1:], optimize=not False, duration=100, loop=0 )
    print(' Exported : [%s]'%savepath)

NI_MAX = 10000
images=[]



# Initialize pygame
pygame.init()

# Open a display
srf = pygame.display.set_mode( (W,H) )




MILLI = 0.001

DT = 0.001# 0.001 #0.04
G = 9.81

import ode

# Create a world object
world = ode.World()

world.setGravity( (0,-G,0) )

R = 0.0001 #10.0 *MILLI
mass = 1.0 

ERP_GLOBAL = 1.0 #0.8 #1.0 #0.8 #1.0
CFM_GLOBAL = 1.0e-6 #0.0
COLLISION = not True
BOUNCE = 0.5 

JOINT = True

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 )

bodys = [body0,body1]
RGBs = {body0:(0,0,0), body1:(127,63,63) , None:(63,0,0)}


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()
        joints = [fix0]

        j = j01 = ode.SliderJoint( world ) 
        j.attach( body0, body1 )
        j.setAxis( (0,1,0) )
        joints.append( j01 )

        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

        kp_ = 0.5 *KP #20.0    ####    Spring-rate [N/m]
        kd_ = 0.5 *KD #8.944 * 0.01 #0.25 #0.0  #4.45 #8.9    ####    Damping-rate

        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)

        j = j01_2 = ode.SliderJoint( world ) 
        j.attach( body0, body1 )
        j.setAxis( (0,1,0) )
        joints.append( j01_2 )

        #kp = KP #20.0    ####    Spring-rate [N/m]
        #kd = KD #8.944 * 0.01 #0.25 #0.0  #4.45 #8.9    ####    Damping-rate

        #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):
        """ 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) )

#dt = DT #1.0/fps
fps = FPS #30.0
loopFlag = True
clk = pygame.time.Clock()

END_TIME = 5.0

PRNT = False
tn=-1
while loopFlag and total_time <= END_TIME:
    tn += 1
    events = pygame.event.get()
    for e in events:
        if e.type==QUIT:
            loopFlag=False

        if e.type==KEYDOWN:

            loopFlag=False

    # Clear the screen
    srf.fill((255,255,255))


    for joint in joints:
        #print('type(joint) = ',type(joint) )
        #if joint is j0:
        #    continue



        rgb = (127,127,127)
        b_move = None

        b0 = joint.getBody(0)
        b1 = joint.getBody(1)

        if PRNT:
            print('b0 = ', b0 )
            print('b1 = ', b1 )

        if type(joint) == ode.FixedJoint or type(joint) == ode.SliderJoint:
           #continue
            if None in [b0,b1]: 
                continue
            p0 = coord( b0.getPosition() )
            p1 = coord( b1.getPosition() )

        elif type(joint) == ode.HingeJoint:
            if b0:
                if b0.getPosition() != joint.getAnchor():
                    b_move = b0
            if None is b0:
                p0 = coord( joint.getAnchor() )
                if None is p0:
                    p0 = coord( body0.getPosition() )
            else:
                p0 = coord( b0.getPosition() )

            if b1:
                if b1.getPosition() != joint.getAnchor():
                    b_move = b0
            if None is b1:
                p1 = coord( joint.getAnchor() )
                if None is p1:
                    p1 = coord( body0.getPosition() )
            else:
                p1 = coord( b1.getPosition() )
        lw = 5

        rgb = RGBs[b_move]
        if PRNT:
            print('p0=',p0)
            print('p1=',p1)
            print( flush=True)

        if( None is not p0 and None is not p1):
            pygame.draw.line( srf, rgb, p0,p1, lw)

    for body in bodys:
        xyz = body.getPosition()
        rgb = RGBs[body] #(127,127,127)
        pygame.draw.circle(srf, rgb, coord( xyz ), 20, 0)

    pygame.display.flip()

    if tn % RENDERING_INTERVAL == 0:
        storeImage(srf,images)






    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 or JOINT:
        # Detect collisions and create contact joints
        space.collide( (world,contactgroup), near_callback )

    world.step(dt)
    # Try to keep the specified framerate    
    if KEEP_FPS:
        clk.tick(fps)

    total_time+=dt

    if COLLISION:
        # Remove all contact joints
        contactgroup.empty()


    #tn += 1

end_tn = tn


if GIF:
    gif( images )





########        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)