[PYTHON] Spring damper with PyODE

I tried to implement a spring damper system with PyODE. So I checked if it works according to the physical properties.

1 Overview

Sites that I referred to without permission → https://w.atwiki.jp/bambooflow/pages/242.html

There are no springs or dampers in ODE. Therefore, the spring constant and damping factor are replaced with the values of ERP and CFM, which are the mutual constraint conditions for objects. (It seems to be written in the ODE manual.) In this article, we will connect two objects with slider joints and set ERP and CFM. ↓ This is the relevant partial code.

ODE spring damper setting method (using slider)


        h = DT = 0.01    ###Let h be the time step length
        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) )        ####A slider that moves only in the Y-axis direction.

        j.setParam(ode.ParamLoStop, 0.0)    ####This is the standard on which springs and dampers work.
        j.setParam(ode.ParamHiStop, 0.0)    ####The standard on which springs and dampers work.
        
        j.setParam(ode.ParamStopERP, erp)
        j.setParam(ode.ParamStopCFM, cfm)

2 Code (whole description)

Previous article → "Sample of PyODE contact judgment and bound behavior" (https://qiita.com/emuai/items/652d36fb19b6f41dcd38) It was repaired based on the code of.

Code overview

・ 2 objects (no floor required) -Place the first object `` body0``` at a height of 0 (*** Strictly 0 was a malfunction **, so it was invisible and shifted to a height of 1 mm). .. -Fixed body0 to the environment. -Place the second object ``` body1``` in the sky 1m above body0. -Connect body0 and` `body1 with a slider ・ Set the ERP / CFM value corresponding to the spring and damper on the slider. -After the calculation is completed, a logical solution that matches the calculation conditions is generated and compared with the result for plotting.

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

The main purpose is to compare with the logical solution. I've commented it out, but now you can see plots with zeta = 0.0, 0.5, 1.0, 1.5.

3 Results

I plotted the height of the moving object at 1m above the sky ( Y = 1). Since the mass is 1 kg and the spring constant is 20 [kg / m] under gravity, it should vibrate around Y = 0.5 m. When I shook the attenuation rate `KD```, it sometimes deviated from the logical solution depending on the time step length `` DT```. Here, I will show you two calculation results.

↓ First, it is the result of the time step length `` `DT = 0.01```. DT = 0.01 y_DT0.01_kp20_kd0.08944_zeta0.0099997.png At DT = 0.01, it does not match the logical solution (thick line). The calculation is too rough.

So reduce the DT by a factor of 10. DT = 0.001 y_DT0.001_kp20_kd0.08944_zeta0.0099997.png It has the same physical properties Thus, `` `DT = 0.001``` was normal.

4 Summary

It should be noted that it does not move according to the physical properties depending on the time. ・ By the way, it seems that the time step is rougher under the condition that the viscosity is much stronger than the above example and the vibration does not appear and converges.

ODE's obscure behavior

Where I trapped this time: -Fixed objects (" body0``` "in the code) do not seem to rest as set unless they are placed higher than `0.0``` (?)

What is it? In any case, if you have to do something like "Plot and check fixed objects", you may get stuck in a pot.

Recommended Posts

Spring damper with PyODE
Non-linear spring with PyODE slider
Access Azure Cosmos DB with Spring Boot