PyODE スフィア連結オブジェクト

こちらにPyODEのマニュアル等の情報など → https://qiita.com/emuai/items/dac5f9c6e9ea5e70191e

PyODE の球オブジェクトを連結して棒状のオブジェクトを作ります。
* 球連結棒は質量の配分を調整できます。
* 素直に棒オブジェクトを使うよりも処理量は少ない筈です。

## 1 動機
ポリゴンオブジェクトより手軽な気がするので、この方法を試します。
サンプルとして置いときます。（あまり整理してないコードですが汗）

## 2 サンプルコード
早速ですがコード晒します。
3つのオブジェクトをFixedJointで連結し、
地面に落とします。
両端のオブジェクトのみ、地面との衝突を計算します。


```

##  #!C:\Python380\python.exe -i .\takeuma_0.py


##  pyODE example-1: with MPL-plot
##  Appended ``A floor and collision'' to observe bouncing motion.


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 = 720 #640 #320 #640
H = 320 #640
CX = 100 #320
CY = 280 #500 #320
S = 100. #200.0
def coord(xyz):
    #(x,y,z) = xyz
    (x,_,y) = 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,savepath='takeuma.gif'):
    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)
    
    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) )






DT = 0.0005# 0.001 #0.04
G = 9.81


MILLI = 0.001

import ode

# Create a world object
world = ode.World()
world.setGravity( (0,0,-G) )


POLE_LENGTH=1.0
XF0=0.0
YF0=0.0
ZF0=0.1 #1.0

xyz1=(XF0,YF0,ZF0)
M1=1.0
R1=0.05 #0.1
xyz2=(XF0+0.1,YF0,ZF0+1.0)
M2=POLE_LENGTH * 1.0 # [kg]
R2=0.1
xyz3=(XF0+0.2,YF0,ZF0+2.0)
M3=60.
R3=0.1

# Create a body inside the world
body = b1 = body1 = ode.Body(world)
m=m1=ode.Mass()
rho = 2700.0  ##  AL??
m.setSphere( rho, R1)
m.mass = M1
body.setMass(m)
body.setPosition( xyz1 )
body.setLinearVel( (0.0,0.0,0.0))  


b=b2=body2=ode.Body(world)
m=m2=ode.Mass()
#rho = 2700.0  ##  AL??
#r = 10.0 *MILLI #0.001 #0.05
m.setSphere( rho, R2)
m.mass = M2 
b.setMass(m)
b.setPosition( xyz2 )
b.setLinearVel( (0.0,0.0,0.0))  



b=b3=body3=ode.Body(world)
m=m3=ode.Mass()
#rho = 2700.0  ##  AL??
#r = 10.0 *MILLI #0.001 #0.05
m.setSphere( rho, R3)
m.mass = M3 
b.setMass(m)
b.setPosition( xyz3 )
b.setLinearVel( (0.0,0.0,0.0))  




bodys = [b1,b2,b3]
b0=None
RGBs = {b0:(0,0,0), b1:(127,63,63) , b2:(0,63,0), b3:(0,0,63) }





ERP_GLOBAL = 1.0 #0.8 #1.0
CFM_GLOBAL = 0.0
COLLISION = True
BOUNCE = 0.2 # 0.1 #0.5 ##  bouncing rate 
MU=10000 #5000

js = joints = []
if True: #COLLISION:
    # Create a space object
    space = ode.Space()
    # Create a plane geom which prevent the objects from falling forever
    #floor = ode.GeomPlane( space, (0,1,0), 0. )
    floor = ode.GeomPlane( space, (0.,0.,1.), 0. )

    g=g1=geom = ode.GeomSphere( space, radius=R1 )
    g.setBody( b1 )

    g=g3= ode.GeomSphere( space, radius=R3 )
    g.setBody( b3 )

    if False:
        j = fix0 = ode.FixedJoint(world)
        j.attach( body0, ode.environment )
        j.setFixed()
        joints = [fix0]

    j = j12 = ode.FixedJoint( world ) 
    j.attach( b1, b2 )
    #j.setAxis( (0,1,0) )
    j.setFixed()
    joints.append( j12 )

    j = j23 = ode.FixedJoint( world ) 
    j.attach( b2, b3 )
    #j.setAxis( (0,1,0) )
    j.setFixed()
    joints.append( j23 )


    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 ) #0.2)
            c.setMu(MU)#5000)
                        
            j = ode.ContactJoint( world, contactgroup, c )
            j.attach( geom1.getBody(), geom2.getBody() )



##  Proceed the simulation...
total_time = 0.0
dt = 0.001 #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))


    pL=(-10.,0.,0.)
    pR=(10.,0.,0.)
    lw=10
    rgb=(0,0,0)
    pygame.draw.line(srf,rgb, coord(pL), coord(pR) ,lw  )

    for body in bodys:
        xyz = body.getPosition()
        rgb = RGBs[body] #(127,127,127)
        pygame.draw.circle(srf, rgb, coord( xyz ), 20, 0)
    


    p0 = ( b1.getPosition() )
    p1 = ( b3.getPosition() )
    p0 = coord( p0 )
    p1 = coord( p1 )
    lw=30
    rgb=(127,127,127)
    pygame.draw.line( srf, rgb, p0,p1, lw)


    pygame.display.flip()

    if tn % RENDERING_INTERVAL == 0:
        storeImage(srf,images)







    body=b1
    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 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()

    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


end_tn = tn

if GIF:
    gif( images )



import matplotlib.pyplot as plt
# MPL-Plot 
plt.plot( txyzuvw[0][0:end_tn], txyzuvw[3][0:end_tn], label='Vertical position')
plt.plot( txyzuvw[0][0:end_tn], txyzuvw[6][0:end_tn], label='Vertical velocity')

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

xmin = np.min( txyzuvw[0] )
xmax = np.max( txyzuvw[0] )

plt.hlines( [0], xmin, xmax, "blue", linestyles='dashed')     # hlines

savepath = './y_ERP%g_CFM%g_BR%g.png'%(ERP_GLOBAL, CFM_GLOBAL, BOUNCE)
plt.savefig( savepath )
print('An image-file exported : [%s]'%savepath )

#plt.show()
plt.pause(1.0)





```

## 3 結果
Pygameによる可視化です。↓

![takeuma.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/469875/8469d038-c51b-5c5d-5081-72fad010a53f.gif)

それっぽい動き。
※物理として正しいかどうかは未検証です。
※Y軸を使ってませんが3Dシミュレーションです。

## 4 総括
スフィアを連結して棒状のオブジェクトを地面に転がすことができました。
ここからいろいろと遊べそうです。