[PYTHON] Federdämpfer mit PyODE

Ich habe versucht, mit PyODE ein Federdämpfersystem zu implementieren. Damit Ich habe überprüft, ob es gemäß den physikalischen Eigenschaften funktioniert.

1. Übersicht

Eine Site, auf die ich ohne Erlaubnis verwiesen habe → https://w.atwiki.jp/bambooflow/pages/242.html

Es gibt keine Federn oder Dämpfer in ODE. Daher werden die Federkonstante und die Dämpfungsrate durch die Werte von ERP und CFM ersetzt, die die gegenseitigen Randbedingungen für Objekte sind. (Es scheint im ODE-Handbuch geschrieben zu sein.) In diesem Artikel verbinden wir zwei Objekte mit Gleitgelenken und legen ERP und CFM fest. ↓ Dies ist der relevante Teilecode.

ODE-Federdämpfer-Einstellmethode (mit Schieberegler)


        h = DT = 0.01    ###Sei h die Zeitschrittlänge
        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) )        ####Ein Schieberegler, der sich nur in Richtung der Y-Achse bewegt.

        j.setParam(ode.ParamLoStop, 0.0)    ####Dies ist der Standard, an dem Federn und Dämpfer arbeiten.
        j.setParam(ode.ParamHiStop, 0.0)    ####Der Standard, an dem Federn und Dämpfer arbeiten.
        
        j.setParam(ode.ParamStopERP, erp)
        j.setParam(ode.ParamStopCFM, cfm)

2 Code (vollständige Beschreibung)

Vorheriger Artikel → "Beispiel für PyODE-Kontaktbeurteilung und gebundenes Verhalten" (https://qiita.com/emuai/items/652d36fb19b6f41dcd38) Es wurde basierend auf dem Code von repariert.

Codeübersicht

・ 2 Objekte (kein Boden erforderlich)

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

Der Hauptzweck ist der Vergleich mit der logischen Lösung. Ich habe es auskommentiert, aber Sie können jetzt Diagramme mit Zeta = 0,0, 0,5, 1,0, 1,5 auflisten.

3. Ergebnisse

Ich habe die Höhe des sich bewegenden Objekts 1 m über dem Himmel aufgezeichnet (`` `Y = 1```). Da die Masse 1 kg beträgt und der Federkoeffizient unter Schwerkraft 20 [kg / m] beträgt, sollte er um Y = 0,5 m schwingen. Wenn ich versuchte, die Dämpfungsrate "KD" zu schütteln, weicht sie manchmal von der logischen Lösung ab, abhängig von der Zeitschrittlänge "DT". Hier zeige ich Ihnen zwei Berechnungsergebnisse.

↓ Erstens ist es das Ergebnis der Zeitschrittlänge `` `DT = 0.01```. DT = 0.01 y_DT0.01_kp20_kd0.08944_zeta0.0099997.png Bei DT = 0,01 stimmt es nicht mit der logischen Lösung überein (dicke Linie). Die Berechnung ist zu grob.

DT wird also um den Faktor 10 reduziert. DT = 0.001 y_DT0.001_kp20_kd0.08944_zeta0.0099997.png Es hat die gleichen physikalischen Eigenschaften Somit war "DT = 0,001" normal.

4 Zusammenfassung

Es ist zu beachten, dass es sich je nach Zeit nicht entsprechend den physikalischen Eigenschaften bewegt. ・ Übrigens scheint der Zeitschritt unter der Bedingung, dass die Viskosität viel stärker als im obigen Beispiel ist und die Vibration nicht auftritt und konvergiert, rauer zu sein.

Das obskure Verhalten von ODE

Wo ich diesmal gefangen war: -Fixierte Objekte (" body0``` "im Code) scheinen nicht still zu stehen, es sei denn, sie sind höher als `0.0``` (?)

Was ist es? In jedem Fall können Sie in einem Topf stecken bleiben, wenn Sie etwas wie "Plotten und Überprüfen fester Objekte" ausführen müssen.

Recommended Posts

Federdämpfer mit PyODE
Nichtlineare Feder mit PyODE-Schieber
Greifen Sie mit Spring Boot auf Azure Cosmos DB zu