[PYTHON] Essayez de doubler le curseur PyODE

Yuki Kotonari

Dans l'article précédent `` PyODE Spring Damper '' L'amortisseur à ressort a été construit par le curseur ODE (ode.SliderJoint).

Cependant, je veux faire des ressorts composés. Un seul curseur n'a pas beaucoup de sens car il nécessite des connexions équilibrées de ressorts et d'amortisseurs aux propriétés différentes. (Je ne sais toujours pas s'ils devraient être constitués de curseurs.)

J'ai donc essayé de voir si je pouvais doubler le curseur entre les deux objets.

Ce que tu as fait

・ J'ai essayé de mettre deux curseurs entre deux objets ・ Ajustement de la valeur CFM du monde -Ajout de la visualisation de Pygame

Conclusion plus tôt:

-Il était nécessaire d'ajuster la valeur CFM de ** world, mais les doubles diapositives peuvent être configurées. ** **

Ajustement CFM requis

** * Cet ERP / CFM est différent du curseur ERP / CFM. ** **

Quand il y a un curseur

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

Et ainsi de suite, ERP = 1.0, CFM = 0.0```, et cela a fonctionné.

Cependant, je pense que 1.0 et 0.0 sont à l'origine des valeurs déraisonnables. Il a probablement bougé parce qu'il n'y avait qu'une seule articulation et qu'il n'y avait rien qui s'interfère les uns avec les autres.

J'ai ajouté les curseurs à 2 avec les mêmes paramètres et je l'ai exécuté. En même temps que le départ, un message comme celui-ci est apparu et s'est arrêté. On peut dire que s'arrêter est comme prévu. Mais je pense que ce message est hostile. Même si je l'ai lu, je n'ai aucune idée de la cause.

En ce moment, je pensais à ajuster ERP / CFM, donc pour le moment

ERP_GLOBAL = 1.0
CFM_GLOBAL = 1.0e-6 #0.0

Je l'ai changé en. Maintenant ça marche.

Dans cette configuration, il était normal de laisser ERP à 1.0.

Résultat d'exécution

Visualisation 2D avec Pygame. Il suit la méthode de PyODE "Tutorial 2". (Il y a beaucoup de marges, mais je ne pense pas qu'il faudra du temps pour faire de tels ajustements.) ↑ Le cercle noir est un objet fixe et le cercle rouge-brun est connecté au-dessus via deux joints coulissants. ** Les lignes de curseur sont dessinées avec des décalages gauche et droit, mais elles se chevauchent sur l'ODE. ** ** (Le code affiché n'est pas décalé)

La position du cercle rouge-brun par rapport au tracé temporel est sortie comme dans le cas précédent. ↓ ** Ligne bleue: solution logique, ligne orange: calcul ODE, la vitesse sur l'étiquette de l'axe vertical est oubliée pour être effacée **

Au lieu de doubler les curseurs, les coefficients du ressort et de l'amortisseur sont chacun divisés par deux. Ensuite, le résultat est le même que ci-dessus.

Code (tout)

Ça a été beaucoup de désordre Je publierai le tout cette fois également.

La visualisation Pygame a été ajoutée.

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