PyODE 接触判定とバウンド挙動のサンプル

###実施の経緯
本家 PyODE 様サイトは[こちら](http://pyode.sourceforge.net/)　

本家チュートリアルには「接触判定」と「床でバウンド」のチュートリアルとして「Tutorial 3」があります。
しかし、**落下オブジェクトが多数**あったり、**3D描画部分のコード**が多かったり、**OpenGLが必要**だったりと
なんか過剰というか、実施しづらいサンプルです。

そこで本記事では最も単純な「Tutorial 1」に**接触/床反発**を足してみました。

↓　こちらは拙者の関連記事です。
　・[「Tutorial 1」実施記事](https://qiita.com/emuai/items/75df831c16dbe660df68)
　・[「Tutorial 3」実施記事](https://qiita.com/emuai/items/840c776f146ac12ac416)

##1 概要
本家の「Tutorial 1」は、１個のオブジェクトに初速を与えて自由落下の様子を見るものでした。
[先の記事] (https://qiita.com/emuai/items/75df831c16dbe660df68)では、Matplotlib でプロット表示追加して実施しました。
このときのプログラムに
　・オブジェクトに接触判定用形状定義
　・床の定義
　・接触判定ルーチン
の3つを追加します。
これら追加部分は、前述「本家Tutorial 3」に倣っています。


##2 サンプルコード（全部）
定数```COLLISION``` でスイッチするところが追加部分です。

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

import ode

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


# Create a body inside the world
body = ode.Body(world)
M = ode.Mass()
rho = 2500.0
r = 0.05
M.setSphere( rho, r)
M.mass = 1.0
body.setMass(M)

body.setPosition( (0,0,0) )
#body.addForce( (0,200,0) )
body.setLinearVel( (0.0,10.0,0.0))  


ERP_GLOBAL = 1.0
CFM_GLOBAL = 0.0
COLLISION = True
BOUNCE = 0.5 

if 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. )

    geom = ode.GeomSphere( space, radius=r )
    geom.setBody( body )

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



##  Proceed the simulation...
total_time = 0.0
dt = 0.04

import numpy as np
nt = 100
txyzuvw = np.zeros( (7,nt+1) )
tn=0

END_TIME = 5.0
while total_time <= END_TIME:
    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

import matplotlib.pyplot as plt
# MPL-Plot 
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.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)

```
↑　20200506:　スタートの位置記録できてなかったの修正

```BOUNCE = 0.5``` が反発係数です。

##3 プログラム実行と結果
　(あらかじめnumpyとmatplotlibをインストールしてください。)
実行すると......
![y_ERP1_CFM0_BR0.5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/469875/6a1452dc-bdac-6cf2-3d3a-32f14ef7639f.png)
↑　このグラフ画像(PNG)がカレントに保存されます。
垂直方向（Y座標）に投じられた物体の高さと速度の時間変化です。
接触判定に使われる物体直径値は0.1mです。（もっと小さくてもよかった。）
落ちてきたオブジェクトは ```-10.0[m/s]``` で床に衝突し```+5.0 [m/s]``` で反発しているから
1回目のバウンドは係数どおりの挙動です。
2回目のバウンドはかなりずれている。
これは```dt```の値によっても変わってきますし、ERP、CFMによっても変わるはず。

---

```python
    world.setERP( ERP_GLOBAL )   ####    ERP : Error Reduction Parameter 
    world.setCFM( CFM_GLOBAL )   ####    CFM : Constraint Force Mixing
```
↑この部分でセットする2つの値は、もっと高度な計算の際は検討が必要。
今回の値```1.0``` と ```0.0``` は弾性衝突の場合の理想的な値です。
おそらく疑似的に柔らかい物性にするとか、計算コストの都合とかで調整する
（ものだと筆者はみている、まだ厳密には把握してない汗）