けん玉の運動シミュレーション

#目的
けん玉をロボットにさせたいと思っており、そのためのシミュレーション実験を行うために、まず運動シミュレーションを作成しました。
とりあえず、「けん」は動かさない状態でのシミュレーションをします。

難しい点

* 紐につながれているため、張力が働いているときは円運動をしますが、張力が0の時は自由落下として運動します。この条件分岐が難しい

今回は単純にオイラー法で行います。

#運動方程式
円運動時と、自由落下時の２つあります。

円運動時

```math
ma = (0,-mg)+ T(-cos\theta,sin\theta)\\
a = \frac{d^2\theta}{dt^2} l(-sin\theta,cos\theta) + (\frac{d\theta}{dt})^2 l(-cos\theta,sin\theta)\\
```

落下時

```math
ma = (0,-mg)\\
```

二つの切り替えをどのようにするか悩ましいです。以下で実際にシミュレーションしながら試しています。

#シミュレーション分岐

###分岐なし、完全円運動
![first.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/aca9cb89-0193-d659-d765-f6c2bd103ca3.gif)

それっぽい運動をしています。しかし、実際の運動では最初は円運動ではなく、直線的に下に落下するはずです（なぜなら、紐というたわむことが可能な素材で中心とつながっているため）。

###分岐1
張力が

* 正の方向 -> 円運動
* 負の方向 -> 落下運動

という考え方で行ってみます。

```python
for i in range(N):
   """
　　現在の角度、角速度から　角加速度・張力を求める
   theta   角度
   dtheta  角速度
　 ddtheta 各加速度
   T　　　　張力   
   """
    A=np.array([[-1*m*l*np.sin(theta), np.cos(theta)],[-1*m*l*np.cos(theta),-1*np.sin(theta)]])
    hoge = m*(dtheta*dtheta)*l    
    B=np.array([[hoge*np.cos(theta)],[hoge*-1*np.sin(theta)-m*g]])
    
    ans = np.linalg.solve(A,B)
    
    ddtheta =  ans[0][0]
    T =  ans[1][0]
    
    ddtheta =  ans[0][0]
    T =  ans[1][0]
    
   
    if T >= 0:
        """
        円運動に従う
        """
        #更新
        theta = theta + dtheta*dt
        dtheta = dtheta + ddtheta*dt
        x = l*np.cos(theta)
        y = -1*l*np.sin(theta)
        vx = -1*dtheta*l*np.sin(theta)
        vy = -1*dtheta*l*np.cos(theta)
        
    else:
        
        """
        mx = -mg に従う
        """
        #更新
        vy = vy -m*g*dt
        x = x + vx*dt
        y = y + vy*dt
        theta = math.atan(-1*y/x)
        dtheta = math.sqrt((vx*vx+vy*vy)/(l*l))
        if vy/math.sin(theta) > 0:
            dtheta=-1*dtheta
```
実行結果
![second.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/88f853aa-a956-54ef-48d2-0367a0479e95.gif)

問題

* 急に位置が吹っ飛ぶ
* エネルギーが増加している（一回転しているので）

なぜ、位置が吹っ飛ぶかというと

落下時の、角度、角速度の更新がよくないのが原因です。

* 落下時は、速度、x-y座標を時間更新
* 回転時は、角度、角速度を時間更新

しています。
角度と角速度から速度、x-y座標を常時求めることはできます。
しかし、落下時には角速度という概念は存在しません。上の計算では、仮に円運動を行っていると仮定した際の速度と角速度の関係から角速度を求めています。角度は$atan$で完全にわかります。

よって、落下運動時に計算される張力の数値、角速度の数値が怪しいため、正しい張力を推定できているか不明です。

「急に位置が吹っ飛ぶ」のは、落下運動中に$T>=0$の判定に引っ掛かり、円運動軌道に乗っていると見えます。

###分岐2
紐の長さを考慮に入れる。
張力のみでは、判定を間違えてしまったので、場当たり的ですが、紐の長さを条件に入れてみます。

紐の長さが元の

* 90%以下なら落下とみなし
* それ以上なら、円運動と仮定した際の張力を計算し、張力の正負で判定する

シミュレーション結果

![second.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/d7a53c13-45d9-0570-6795-224ce6e69e66.gif)

結構きれいに、落下と回転の運動が別々にシミュレーションできています。

コード

```python
for i in range(N):
    length_x = x*x+ y*y #紐の長さ計算
    
    if length_x > l*0.9:
    
        A=np.array([[-1*m*l*np.sin(theta), np.cos(theta)],[-1*m*l*np.cos(theta),-1*np.sin(theta)]])
        hoge = m*(dtheta*dtheta)*l    
        B=np.array([[hoge*np.cos(theta)],[hoge*-1*np.sin(theta)-m*g]])
        
        ans = np.linalg.solve(A,B)
        
        ddtheta =  ans[0][0]
        T =  ans[1][0]
    else:
        T = -10   # 落下運動確定
        
    if T >= 0:
        
        #更新
        theta = theta + dtheta*dt
        dtheta = dtheta + ddtheta*dt
        x = l*np.cos(theta)
        y = -1*l*np.sin(theta)
        vx = -1*dtheta*l*np.sin(theta)
        vy = -1*dtheta*l*np.cos(theta)
        
    else:

        """
        mx = -mg に従う
        """
        #更新
        vy = vy -m*g*dt
        x = x + vx*dt
        y = y + vy*dt
        
        
        
        theta = math.atan2(-1*y,x)
        if theta<0:
            theta = theta+math.pi*2
            
        dtheta = math.sqrt((vx*vx+vy*vy)/(l*l))
        if vx/math.sin(theta) > 0:
            dtheta=-1*dtheta
        
```

しかし、この方法も問題があります。
初期角度-5度で落下させています。
![second.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/7962a551-f156-6160-8126-f2931500a12e.gif)

なぜこうなるかというと
$vx,vy$から$\theta ,\frac{d\theta}{dt}$への変換がよくないためと考えられます。

###分岐3
$vx,vy$から$\theta ,\frac{d\theta}{dt}$への変換を変更。
エネルギーが大きく失われる。


```python
        theta = math.atan2(-1*y,x)
        if theta<0:
            theta = theta+math.pi*2
            
        A=np.array([[-1*l*np.sin(theta),-1* np.cos(theta)],[-1*l*np.cos(theta),np.sin(theta)]])
        B=np.array([[vx],[vy]])
        
        ans = np.linalg.solve(A,B)
        dtheta = ans[0][0]
```
で変換

![second.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/51054530-c636-f1e7-44f5-788f0dc714b5.gif)

わるくはないが、落下から回転に切り替わるときの挙動がちょっとおかしい？


難しいなと思いました。

ちょっと変更し、

紐の長さが元の

* 100%以下なら落下とみなし(前回のは90%)

ということにしてみます。

![second.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/fc18a64b-e2f9-c6e0-f473-5009692f35ec.gif)
先ほどより、円運動と落下の切り替えがスムーズになりました。

完璧です（ということにする）


#最終コード

```python
import numpy as np
import matplotlib.pyplot as plt
import cv2			#	OpenCVライブラリ
import math


from PIL import Image, ImageDraw
def cv2pil(image):
    ''' OpenCV型 -> PIL型 '''
    new_image = image.copy()
    if new_image.ndim == 2:  # モノクロ
        pass
    elif new_image.shape[2] == 3:  # カラー
        new_image = cv2.cvtColor(new_image, cv2.COLOR_BGR2RGB)
    elif new_image.shape[2] == 4:  # 透過
        new_image = cv2.cvtColor(new_image, cv2.COLOR_BGRA2RGBA)
    new_image = Image.fromarray(new_image)
    return new_image

m=1
l=1
theta = -30*np.pi/180
T=0
dtheta = 0
g=9.81


dt = 0.001
N = 10000

x = l*np.cos(theta)
y = -1*l*np.sin(theta)
vx = -1*dtheta*l*np.sin(theta)
vy = -1*dtheta*l*np.cos(theta)

x_vec = np.array([[x],[y]])
save_x = np.array(x_vec)
v_vec = np.array([[vx],[vy]])
save_v = np.array(v_vec)
theta_vec = np.array([[dtheta],[theta]])
save_theta = np.array(theta_vec)

energy = (vx*vx+vy*vy)/2*m + m*y*g
save_energy = np.array([energy])

save_T = np.array([0])

for i in range(N):
    length_x = x*x+ y*y #紐の長さ計算6
    
    if length_x > l*1.0:
    
        A=np.array([[-1*m*l*np.sin(theta), np.cos(theta)],[-1*m*l*np.cos(theta),-1*np.sin(theta)]])
        hoge = m*(dtheta*dtheta)*l    
        B=np.array([[hoge*np.cos(theta)],[hoge*-1*np.sin(theta)-m*g]])
        
        ans = np.linalg.solve(A,B)
        
        ddtheta =  ans[0][0]
        T =  ans[1][0]
    else:
        T = -10   # 落下運動確定
        
    if T >= 0:
        
        #更新
        theta = theta + dtheta*dt
        dtheta = dtheta + ddtheta*dt
        x = l*np.cos(theta)
        y = -1*l*np.sin(theta)
        vx = -1*dtheta*l*np.sin(theta)
        vy = -1*dtheta*l*np.cos(theta)
        
    else:

        """
        mx = -mg に従う
        """
        #更新
        vy = vy -m*g*dt
        x = x + vx*dt
        y = y + vy*dt
        
        
        
        theta = math.atan2(-1*y,x)
        if theta<0:
            theta = theta+math.pi*2
            
        A=np.array([[-1*l*np.sin(theta),-1* np.cos(theta)],[-1*l*np.cos(theta),np.sin(theta)]])
        B=np.array([[vx],[vy]])
        
        ans = np.linalg.solve(A,B)
        dtheta = ans[0][0]
        
    #保存
    x_vec = np.array([[x],[y]])
    save_x = np.append(save_x,x_vec,1)
    theta_vec = np.array([[dtheta],[theta]])
    save_theta = np.append(save_theta,theta_vec,1)
    energy = (vx*vx+vy*vy)/2*m + m*y*g
    save_energy = np.append(save_energy,energy)
    save_T = np.append(save_T,T)
    v_vec = np.array([[vx],[vy]])
    save_v = np.append(save_v,v_vec,1)
    

#グラフ化
t=np.arange(N+1)*dt   

fig = plt.figure(figsize=(4, 3)) #...1
ax = fig.add_subplot(511) #...2
ax2 = fig.add_subplot(512) #...
ax3 = fig.add_subplot(513) #...2
ax4 = fig.add_subplot(514) #...2
ax5 = fig.add_subplot(515) #...2

ax.plot(t,save_x[0,:],"r",label="x")
ax.plot(t,save_x[1,:],"k",label="y")

ax2.plot(t,save_theta[1,:])

ax3.plot(t,save_energy,"b",label="ene")

ax4.plot(t,save_T,"b",label="T")

ax5.plot(t,save_v[0,:],"r",label="x")
ax5.plot(t,save_v[1,:],"k",label="y")

plt.show()

#グラフィック表示

windowName = "img" #でないとpython3ではだめ
cv2.namedWindow(windowName)

img = cv2.imread('background.png' ,cv2.IMREAD_COLOR)

height,width,channels = img.shape

cv2.line(img,(0,int(height/2)),(width,int(height/2)),(0,0,255),2)
cv2.line(img,(int(width/2),0),(int(width/2),height),(0,0,255),2)

cv2.imshow(windowName,img)    

images = []

frame_step = 10
while True:
    for i in range(int((N+1)/frame_step)):
        img2 = img.copy()
        extend = 300
        px = int(save_x[0][i*frame_step]*extend)
        py = int(save_x[1][i*frame_step]*extend)
        cv2.circle(img2, (int(width/2)+px,int(height/2)-py), 10, (0, 0, 255), 4)
        cv2.imshow(windowName,img2)
        key = cv2.waitKey(10)
        pil_img = cv2pil(img2)
        images.append(pil_img)
        if key ==ord('q'):                 #       end 処理
            break
    
    key = cv2.waitKey(0)
    if key ==ord('q'):                 #       end 処理
        break
        
cv2.waitKey(0)
cv2.destroyAllWindows()

images[0].save('second.gif',
               save_all=True, append_images=images[1:], optimize=False, duration=40, loop=0)


```