PyODE 슬라이더를 이중으로 설정해 보십시오

일의 진전

이전 기사폰 PyODE 스프링 댐퍼'에서
ODE 슬라이더(ode.SliderJoint)를 통해 스프링 댐퍼를 구성합니다.
하지만 필자는 복합형 스프링을 만들고 싶다.
서로 다른 성질의 용수철과 감진기는 균형 있게 연결되어야 하기 때문에 슬라이더 하나는 아무런 의미가 없다.
(이것들이 슬라이더로 구성되어야 하는지 모르겠습니다.)
그래서 나는 두 대상 사이의 슬라이더를 겹칠 수 있는지 없는지를 시험해 보았다.

실시된 일

• 두 객체 사이에 슬라이더 두 개를 삽입해 보십시오
・월드의 CFM치 조정
• Pygame 시각화 추가

먼저 결론을 얻다.

• 월드 CFM 수치를 조정해야 하지만 슬라이딩 이중으로 구성할 수 있습니다.

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은 본래 불합리한 값이라고 생각한다.
이음매가 하나밖에 없어서 서로 간섭할 게 없으니까 움직이는 거지.
원래의 설정에 슬라이더를 더해서 2개로 설정하고 실행하면
시작과 동시에 이런 메시지를 보내고 멈췄다.

주차 자체가 여전하다고 할 수 있다.
하지만 나는 이 소식이 불친절하다고 생각한다.읽어도 무슨 영문인지 전혀 모르겠다.
ERP/CFM의 조정 때문에

ERP_GLOBAL = 1.0
CFM_GLOBAL = 1.0e-6 #0.0

됐어.움직일 거야.
이번 구성에서는 ERP가 1.0을 유지하더라도 문제가 없습니다.

실행 결과

Pygame은 2D 시각화를 구현했습니다.
PyODE "Tutorial 2"패러디.
(공백이 많지만 이런 조정은 시간이 걸리지 않을 것 같다.)

↑ 흑환은 고정 대상이며 그 위에 두 개의 슬라이더 조인트를 통해 홍차환을 연결한다.
슬라이더의 선은 좌우로 오프셋되어 그려지고 ODE에서 중첩됩니다.
(게재 코드가 오프셋되지 않은 상태)
홍차환의 위치 vs 시간도는 이전 상황과 같이 출력됩니다.↓

청선: 논이해, 오렌지선: ODE 계산, 세로축 라벨에 벨로시티 삭제 잊어버리기
용수철, 감진기의 계수를 슬라이더를 중첩시키지 않고 각각 원래의 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)