[PYTHON] Try doubling the PyODE slider

Yuki Kotonari

In the previous article `` PyODE Spring Damper'' The spring damper was constructed by the ODE slider (ode.SliderJoint).

However, I want to do compound springs. A single slider does not make much sense because it requires balanced connections of springs and dampers with different properties. (I'm still not sure if they should consist of sliders.)

So I tried to see if I could double the slider between the two objects.

What you did

・ I tried putting two sliders between two objects ・ CFM value adjustment of world -Added visualization of Pygame

Conclusion earlier:

-It was necessary to adjust the CFM value of ** world, but double slides can be configured. ** **

CFM adjustment required

** * This ERP / CFM is different from the slider ERP / CFM. ** **

When there is one slider

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

And so on, ERP = 1.0, `` `CFM = 0.0```, and it worked.

However, I think that 1.0 and 0.0 are originally unreasonable values. It probably moved because there was only one joint and there was nothing to interfere with each other.

When I added the sliders to 2 with the same settings and executed it At the same time as the start, this message came out and stopped. ODE_INTERNAL_ERROR_1.png It can be said that stopping itself is as expected. But I think this message is unfriendly. Even if I read it, I have no idea what the cause is.

Right now, I was thinking about adjusting ERP / CFM, so for the time being

ERP_GLOBAL = 1.0
CFM_GLOBAL = 1.0e-6 #0.0

I changed it to. Now it works.

With this configuration, it was okay to leave ERP at 1.0.

Execution result

2D visualization with Pygame. It follows the method of PyODE "Tutorial 2". (There are a lot of margins, but I don't think it will take time to make such adjustments.) tutorial_1.gif ↑ The black circle is a fixed object, and the red-brown circle is connected above it via two Slider Joints. ** The slider lines are offset to the left and right, but they overlap on the ODE. ** ** (The posted code is not offset)

The position of the red-brown circle vs. the time plot is output as in the previous case. ↓ y_DT0.0005_kp20_kd0.08944_zeta0.0099997.png ** Blue line: Logical solution, Orange line: ODE calculation, Velocity on the vertical axis label is forgotten to be erased **

Instead of doubling the sliders, the coefficients of the springs and dampers are each halved. Then, the result is the same as above.

Code (all)

It's been a lot of mess I will post the whole thing this time as well.

Added visualization of 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)

Recommended Posts

Try doubling the PyODE slider
Try using PyODE
Try using the Twitter API
Try the Taxii server (1. Server settings)
Try using the Twitter API
Try using the PeeringDB 2.0 API
Non-linear spring with PyODE slider
Try the Python LINE Pay SDK
Try to introduce the theme to Pelican
Try using the Python Cmd module
Cython to try in the shortest
The easiest way to try PyQtGraph