有限要素法の多点拘束機能をPythonで実装する

# 概要
有限要素法のソルバには多点拘束(MPC : Multi Point Constraint)機能が実装されていることがあります。これは主に斜面の拘束や同一平面上に節点を拘束したいときなどに使われる機能で、一般的にはラグランジュの未定乗数法や、自由度消去法、ペナルティ法などで解かれます。今回はラグランジュの未定乗数法を使用してPythonで実装を行ってみました。ただし、2次元限定です。

# 多点拘束の条件式
多点拘束とは複数の節点自由度によって決まる拘束のことを言います。言葉で表すと少し難しくなるので、実例を踏まえて拘束の条件を見ていきます。

<img width="400" alt="坂" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/762468/110de181-e4b9-02cc-4804-7b7acf12c167.png">



例えば上の図のように坂にある物体を斜めに拘束する場合を考えます。
このときに、坂に平行になるように$x^{\prime}$軸を置いた$x^{\prime}y^{\prime}$座標軸を考えると、$x^{\prime}y^{\prime}$座標軸での変位$u^{\prime}$, $v^{\prime}$は以下のように表すことができます。

```math
\begin{bmatrix}
u^{\prime} \\
v^{\prime}
\end{bmatrix}
=
\begin{bmatrix}
cos\theta && sin\theta\\
-sin\theta && cos\theta
\end{bmatrix}
\begin{bmatrix}
u \\
v
\end{bmatrix}
\\
```

ここで, $v^{\prime} =0$なので、

```math
v^{\prime} = - u sin\theta + v cos\theta = 0
```

となり、上の式が斜面上での拘束条件式となります。

<img width="400" alt="強制変位" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/762468/8dc9642d-1359-4a5b-22a4-0ddf913ce2be.png">



次に、上の図のように節点1がx方向に強制変位$\bar{u}$で動くときに節点2から節点Nも同様に動いて欲しい場合を考えてみます。すると、条件式は下記の式で表されます。

```math
u_1 = \bar{u} \\
u_i - u_1 = 0 \quad (i =2,3 \cdots N)
```
<img width="400" alt="不整合" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/762468/a05e54f3-df30-3af0-9475-e33f9f1b4d36.png">

次に、上の図のように2次四角形要素と1次四角形要素が結合している場合を考えます。
ここで、節点2は1次四角形要素に結合していないため、節点2から1次四角形要素に力を伝えることができず、不整合を生じます。
そこで、節点2の変位が節点1,3の平均となるように拘束条件を設けて解析することを考えます。
すると、条件式は下記のようにおくことができます。

```math
u_2 = 0.5u_1 + 0.5u_3 \\
v_2 = 0.5v_1 + 0.5v_3 \\
u_2 - 0.5u_1 - 0.5u_3 = 0 \\
v_2 - 0.5v_1 - 0.5v_3 = 0 \\
```

上記の条件式は全て複数個の節点変位によって表すことができることが分かります。そこで、上記の条件式を一般化して下記のように表してみます。

```math
\begin{bmatrix}
C_{11} && C_{12} && \cdots && C_{1, K}\\
\vdots && \vdots && \ddots && \vdots\\
C_{M, 1} && C_{M, 2} && \cdots && C_{M, K}
\end{bmatrix}
\begin{bmatrix}
u_1 \\
v_1 \\
w_1 \\
\vdots \\
v_{N}\\
w_{N}
\end{bmatrix}
=
\begin{bmatrix}
d_1\\
d_2\\
\vdots \\
d_{3N}
\end{bmatrix}
\\
Cu=d \\
C=
\begin{bmatrix}
C_{11} && C_{12} && \cdots && C_{1, K}\\
\vdots && \vdots && \ddots && \vdots\\
C_{M, 1} && C_{M, 2} && \cdots && C_{M, K}
\end{bmatrix}
,
u=
\begin{bmatrix}
u_1 \\
v_1 \\
w_1 \\
\vdots \\
v_{N}\\
w_{N}
\end{bmatrix}
,
d=
\begin{bmatrix}
d_1\\
d_2\\
\vdots \\
d_{3N}
\end{bmatrix}
\\
u : x方向の変位　\\
v : y方向の変位　\\
w : z方向の変位　\\
```

ここで、$C$マトリクスは拘束条件の各係数をとった横長の行列で、$N$は節点の数、$M$はMPC条件式の数、$K$は全節点の自由度の個数を表しています(K=N×1節点の自由度の数)。
ここで、最初の坂の例を一般化された条件式で表してみると下記のようになります。

<img width="400" alt="坂2" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/762468/4bf47b20-6dda-8196-31cb-f24e321c9429.png">

```math
Cu=d \\
\begin{bmatrix}
-sin\theta && cos\theta && 0 && 0 && 0 && 0 && 0 && 0\\
0 && 0 && -sin\theta && cos\theta && 0 && 0 && 0 && 0
\end{bmatrix}
\begin{bmatrix}
u^1 \\
v^1 \\
u^2 \\
v^2 \\
u^3 \\
v^3 \\
u^4 \\
v^4 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0
\end{bmatrix}
```

このように、多点拘束の条件式は行列を使って$Cu=d$の形で簡単に表すことができます。

# ラグランジュの未定乗数法で変位ベクトルを求める

有限要素法では、線形の弾性解析の場合、下記の式を解くことで変位$u$を求めます。

```math
Ku = f \\
K : 剛性マトリクス\\
u : 変位\\
f : 荷重\\
```

上記の式を多点拘束条件$Cu=d$のもとで解き、$u$を求めることを考えます。
上記の式の導出に関しては他のサイトなどで既に解説されていることが多いので、説明などは省略します。

有限要素法参考リンク
・[有限要素法詳細](https://www.fem-vandv.net/c4.html)
・[有限要素法解析Part 1：仮想仕事の原理 - YouTube](https://www.youtube.com/watch?v=mI7hKh9aDvw&ab_channel=%E3%82%86%E3%81%A3%E3%81%8F%E3%82%8A%E3%81%AE%E3%83%AD%E3%82%B8%E3%82%AB%E3%83%AB%E6%A9%9F%E6%A2%B0%E5%B7%A5%E5%AD%A6)

ここで、$L$を下記のようにおいてみます。

```math
L=\frac{1}{2}u^TKu - u^TF
```

そして、行列のベクトル微分の公式([「ベクトルで微分・行列で微分」公式まとめ](https://qiita.com/AnchorBlues/items/8fe2483a3a72676eb96d))を用いて$L$を$u$ベクトルで偏微分し、ゼロベクトルとおくと、

```math
\frac{\partial L}{\partial u}= Ku - F = 0 \\
Ku = F
```

となります。この式から$L$の極値を求めることは$Ku=F$の式を解くことと同等ということが分かります。
したがって、この問題は多点拘束の条件式$Cu=d$の条件下で$L$の極値を求めるラグランジュの未定乗数法に帰着することができます。

```math
Minimize \,:\, L(u)=\frac{1}{2}u^TKu - u^TF \\
Subject \, To \,:\, g(u) = Cu - d = 0
```

ラグランジュの未定乗数法ではラグランジュ乗数$\lambda$を導入して

```math
\frac{\partial L}{\partial u}= -\lambda\frac{\partial g}{\partial u} \\
```

とおくことができるので、

```math
Ku - F = -\lambda C^T \\
Ku + \lambda C^T = F \\
```

行列形式で制約条件もまとめると、

```math
\left\{
\begin{array}{ll}
Ku + \lambda C^T = F \\
Cu = d \\
\end{array}
\right.
\\
\begin{bmatrix}
K && C^T \\
C && 0 
\end{bmatrix}
\begin{bmatrix}
u \\
\lambda
\end{bmatrix}
=
\begin{bmatrix}
F \\
d
\end{bmatrix}
```

となります。したがって、下記の式を解くことで、MPC条件を満たす変位$u$を求めることができます。

```math
\begin{bmatrix}
u \\
\lambda
\end{bmatrix}
=
\begin{bmatrix}
K && C^T \\
C && 0 
\end{bmatrix}^{-1}
\begin{bmatrix}
F \\
d
\end{bmatrix}
```

# Pythonで実装してみる
今回は下記の例題を解くサンプルをPythonで作成してみます。

<img width="400" alt="例題" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/762468/2a75de1d-4f07-a828-4088-ec09e0c472a3.png">


要素は三角形一次要素2つで、節点1,3を完全拘束、節点2を45°の坂に対して平行となるように拘束します。要素は平面応力状態とし、ヤング率は210000MPa、ポアソン比は0.3、厚さは1mmとします。

### 三角形要素の要素剛性マトリクスの計算
剛性マトリクスは要素毎に存在します。1つの要素の剛性マトリクスは要素剛性マトリクス$K_e$と言われ、i番目の要素剛性マトリクス$K_e^i$を全て足し合わせて全体の剛性マトリクス$K$として計算します。

```math
K = \sum_{i=1}^{N_e} K_e^i \\
N_e : 要素数
```

まずは、一つの三角形要素の剛性マトリクス$K_e$を計算します。
ここで、平面応力状態の応力、ひずみは下記のように表されます。

```math
\begin{bmatrix}
\sigma_x \\
\sigma_y \\
\tau_{xy}
\end{bmatrix}
=
\frac{E}{1-\nu^2}
\begin{bmatrix}
1 && \nu && 0 \\
\nu && 1 && 0 \\
0 && 0 && \frac{1-\nu}{2} \\
\end{bmatrix}
\begin{bmatrix}
\epsilon_x \\
\epsilon_y \\
\gamma_{xy} 
\end{bmatrix}
\\
\sigma_x : x方向の垂直応力 \\
\sigma_y : y方向の垂直応力 \\
\tau_{xy} : せん断応力　\\
E : ヤング率 \\
\nu : ポアソン比 \\
\epsilon_x : x方向の垂直ひずみ \\
\epsilon_y : y方向の垂直ひずみ \\
\gamma_{xy} : 工学せん断ひずみ　\\
```

次に、ひずみと変位は下記の関係で表されます。

```math
\begin{bmatrix}
\epsilon_x \\
\epsilon_y \\
\gamma_{xy} 
\end{bmatrix}
=
\begin{bmatrix}
\frac{\partial u}{\partial x} \\
\frac{\partial v}{\partial y} \\
\frac{\partial u}{\partial y} + \frac{\partial v}{\partial x}
\end{bmatrix}
\\
u : x方向の変位 \\
v : y方向の変位　\\
```

ここで、三角形要素の内の変位$u$, $v$を求めるために、正規化座標系$(a,b)$に座標変換することを考えます。

<img width="600" alt="三角形要素" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/762468/d1d3de07-4b17-838e-938b-68879859b8fd.png">

座標変換は下記の式で与えられます。

```math
\left\{
\begin{array}{ll}
x(a,b) = N_1 x_1 + N_2 x_2 + N_3 x_3 \\
y(a,b) = N_1 y_1 + N_2 y_2 + N_3 y_3 \\
\end{array}
\right. 
\\
N_1 = a \\
N_2 = b \\
N_3 = 1 - a - b
```

$N_1$,$N_2$,$N_3$は形状関数と呼ばれる関数で、$a$,$b$は0～1の値をとります。
すると、要素内の変位は下記の式で表すことができます。

```math
\\
\left\{
\begin{array}{ll}
u(a,b) = N_1 u_1 + N_2 u_2 + N_3 u_3 \\
v(a,b) = N_1 v_1 + N_2 v_2 + N_3 v_3 \\
\end{array}
\right. 
```

この形状関数を使って、ひずみと変位の関係式は下記のように表せます。

```math
\begin{bmatrix}
\epsilon_x \\
\epsilon_y \\
\gamma_{xy} 
\end{bmatrix}
=
\begin{bmatrix}
\frac{\partial u}{\partial x} \\
\frac{\partial v}{\partial y} \\
\frac{\partial u}{\partial y} + \frac{\partial v}{\partial x}
\end{bmatrix}
=
\begin{bmatrix}
\frac{\partial N_1}{\partial x} && 0 && \frac{\partial N_2}{\partial x} && 0 && \frac{\partial N_3}{\partial x} && 0 \\
0 && \frac{\partial N_1}{\partial y} && 0 && \frac{\partial N_2}{\partial y} && 0 && \frac{\partial N_3}{\partial y} \\
\frac{\partial N_1}{\partial y} && \frac{\partial N_1}{\partial x} &&
\frac{\partial N_2}{\partial y} && \frac{\partial N_2}{\partial x} &&
\frac{\partial N_3}{\partial y} && \frac{\partial N_3}{\partial x}
\end{bmatrix}
\begin{bmatrix}
u_1 \\
v_1 \\
u_2 \\
v_2 \\
u_3 \\
v_3
\end{bmatrix}
```

ここで、形状関数$N_i$と$x$,$y$の偏微分が未知なので、ヤコビ行列$J$を使って求めます。

```math
\begin{bmatrix}
\frac{\partial N_i}{\partial a} \\
\frac{\partial N_i}{\partial b}
\end{bmatrix}
=
\begin{bmatrix}
\frac{\partial x}{\partial a} && \frac{\partial y}{\partial a} \\
\frac{\partial x}{\partial b} && \frac{\partial y}{\partial b}
\end{bmatrix}
\begin{bmatrix}
\frac{\partial N_i}{\partial x} \\
\frac{\partial N_i}{\partial y}
\end{bmatrix}
\\
\begin{bmatrix}
\frac{\partial N_i}{\partial a} \\
\frac{\partial N_i}{\partial b}
\end{bmatrix}
=
J
\begin{bmatrix}
\frac{\partial N_i}{\partial x} \\
\frac{\partial N_i}{\partial y}
\end{bmatrix}
\\
\begin{bmatrix}
\frac{\partial N_i}{\partial x} \\
\frac{\partial N_i}{\partial y}
\end{bmatrix}
=
J^{-1}
\begin{bmatrix}
\frac{\partial N_i}{\partial a} \\
\frac{\partial N_i}{\partial b}
\end{bmatrix}
\\
J=
\begin{bmatrix}
\frac{\partial x}{\partial a} && \frac{\partial y}{\partial a} \\
\frac{\partial y}{\partial b} && \frac{\partial y}{\partial b}
\end{bmatrix}
=
\begin{bmatrix}
x_1 - x_3 && y_1 - y_3 \\
 x_2 - x_3 && y_2 - y_3
\end{bmatrix}
```

形状関数は$N_1$~$N_3$まであるので、まとめると下記のように表せます。

```math
\begin{bmatrix}
\frac{\partial N_1}{\partial x} && \frac{\partial N_2}{\partial x} && 
\frac{\partial N_3}{\partial x}\\
\frac{\partial N_1}{\partial y} && \frac{\partial N_2}{\partial y} && 
\frac{\partial N_3}{\partial y}\\
\end{bmatrix}
=
J^{-1}
\begin{bmatrix}
\frac{\partial N_1}{\partial a} && \frac{\partial N_2}{\partial a} && \frac{\partial N_3}{\partial a} \\
\frac{\partial N_1}{\partial b} && \frac{\partial N_2}{\partial b} && \frac{\partial N_3}{\partial b} \\
\end{bmatrix}
=
J^{-1}
\begin{bmatrix}
1 && 0 && -1 \\
0 && 1 && -1 \\
\end{bmatrix}
```

ここで、下記のように置きます。

```math
\sigma = 
\begin{bmatrix}
\sigma_x \\
\sigma_y \\
\tau_{xy}
\end{bmatrix}
\\
\epsilon =
\begin{bmatrix}
\epsilon_x \\
\epsilon_y \\
\gamma_{xy} 
\end{bmatrix}
\\
D = 
\frac{E}{1-\nu^2}
\begin{bmatrix}
1 && \nu && 0 \\
\nu && 1 && 0 \\
0 && 0 && \frac{1-\nu}{2} \\
\end{bmatrix}
\\
B=
\begin{bmatrix}
\frac{\partial N_1}{\partial x} && 0 && \frac{\partial N_2}{\partial x} && 0 && \frac{\partial N_3}{\partial x} && 0 \\
0 && \frac{\partial N_1}{\partial y} && 0 && \frac{\partial N_2}{\partial y} && 0 && \frac{\partial N_3}{\partial y} \\
\frac{\partial N_1}{\partial y} && \frac{\partial N_1}{\partial x} &&
\frac{\partial N_2}{\partial y} && \frac{\partial N_2}{\partial x} &&
\frac{\partial N_3}{\partial y} && \frac{\partial N_3}{\partial x}
\end{bmatrix}
```

すると、要素剛性マトリクス$K_e$は下記のように体積積分の形で表されます。(要素剛性マトリクス$K_e$の導出については上記の有限要素法のリンクを参照ください。)

```math
K_e = \int\int\int B^TDB dV
```

今回は2次元なので、積分を一つ減らすことができます。

```math
K_e = t\int\int B^TDB dxdy \\
t : 要素の厚さ
```

ここで、ヤコビ行列$J$を使って先ほどの正規化座標系に変数変換すると

```math
K_e = t\int_{0}^1\int_{0}^{1-a} B^TDB \begin{vmatrix} J \end{vmatrix} dadb \\
```

となります。上の式はガウス求積を使って求めることが可能です。三角形要素では要素内で$B$マトリクスが一定となるので、積分点は1点だけになります。

```math
K_e = t\int_{0}^1\int_{0}^{1-a} B^TDB \begin{vmatrix} J \end{vmatrix} dadb 
= t w B^T D B \begin{vmatrix} J \end{vmatrix} \\
w = 0.5 : 重み係数
```

したがって、要素剛性マトリクス$K_e$は$B$マトリクス、$D$マトリクス、ヤコビ行列$J$から求めることができることが分かります。ここで、要素を構成する節点は反時計回りに格納されている必要があります。もし、反時計回りでないとヤコビ行列の行列式が負になるため、正常に計算できなくなります。
上記の処理をPythonのクラスで実装してみます。

・Node2dクラス     : 節点の格納用
・Dmatrix2dクラス  : Dマトリクスの計算用
・d2cps3クラス     : 三角形要素の要素剛性マトリクス計算用

節点格納用のクラス : Node2d.py

```Python
class Node2d:

    def __init__(self, no, x, y):
        self.no = no   # 節点番号
        self.x = x     # x座標
        self.y = y     # y座標
```


Dマトリクス計算用クラス : Dmatrix2d.py

```Python
import numpy as np

class Dmatrix2d:

    def __init__(self, young, poisson):
        self.young = young       # ヤング率
        self.poisson = poisson   # ポアソン比
    
    # 平面応力状態のDマトリクスを作成する
    def makeDcpsmatrix(self):
        coef = self.young / (1 - self.poisson * self.poisson)
        matD = np.matrix([[1.0, self.poisson, 0.0],
                          [self.poisson, 1.0, 0.0],
                          [0.0, 0.0, 0.5 * (1 - self.poisson)]])
        matD *= coef

        return matD
```

三角形要素の要素剛性マトリクス計算用クラス : d2cps3.py

```python
import numpy as np
import numpy.linalg as LA
from Dmatrix2d import Dmatrix2d

class d2cps3:

    def __init__(self, no, nodes, thickness, young, poisson):
        self.no = no                 # 要素番号
        self.nodes = nodes           # nodesは反時計回りの順番になっている前提(Node2d型のリスト形式)
        self.thickness = thickness   # 厚さ
        self.young = young           # ヤング率
        self.poisson = poisson       # ポアソン比
        self.ipNum = 1               # 積分点数

        # Dマトリクスを計算する
        self.matD = self.makeDmatrix()

        # ヤコビ行列を計算する
        self.matJ = self.makeJmatrix()

        # Bマトリクスを計算する
        self.matB = self.makeBmatrix()
    
    # Keマトリクスを作成する
    def makeKematrix(self):
        
        w = 0.5   # 積分点の重み係数
        matKe = w * self.thickness * self.matB.T * self.matD * self.matB * LA.det(self.matJ)

        return matKe
    
    # ヤコビ行列を作成する
    def makeJmatrix(self):

        dxda = self.nodes[0].x - self.nodes[2].x
        dxdb = self.nodes[1].x - self.nodes[2].x
        dyda = self.nodes[0].y - self.nodes[2].y
        dydb = self.nodes[1].y - self.nodes[2].y
        matJ = np.matrix([[dxda, dyda],
                          [dxdb, dydb]])
        
        return matJ

    # Dマトリクスを作成する
    def makeDmatrix(self):
        d2dmat = Dmatrix2d(self.young, self.poisson)
        matD = d2dmat.makeDcpsmatrix()

        return matD

    # Bマトリクスを作成する
    def makeBmatrix(self):

        # dNi/da, dNi/dbを計算する
        dN1da = 1.0
        dN1db = 0.0
        dN2da = 0.0
        dN2db = 1.0
        dN3da = -1.0
        dN3db = -1.0

        # dNi/dx, dNi/dyを計算する
        matdNdab = np.matrix([[dN1da, dN2da, dN3da],
                              [dN1db, dN2db, dN3db]])
        #dNdxy = matJinv * matdNdab
        dNdxy = LA.solve(self.matJ, matdNdab)
        
        # Bマトリクスを計算する
        matB = np.matrix([[dNdxy[0, 0], 0.0, dNdxy[0, 1], 0.0, dNdxy[0, 2], 0.0],
                          [0.0, dNdxy[1, 0], 0.0, dNdxy[1, 1], 0.0, dNdxy[1, 2]],
                          [dNdxy[1, 0], dNdxy[0, 0], dNdxy[1, 1], dNdxy[0, 1], dNdxy[1, 2], dNdxy[0, 2]]])
        
        return matB
```



### 全体剛性マトリクスの計算
全体剛性マトリクス$K$は2次元の場合、節点数を$N$とすると下記のように表されます。

```math
Ku=
\begin{bmatrix}
K_{11} && K_{12} && \cdots && K_{1, 2N} \\
K_{21} && K_{22} && \cdots && K_{2, 2N} \\
\vdots && \vdots && \ddots && \vdots \\
K_{2N, 1} && K_{2N, 2} && \cdots && K_{2N, 2N}
\end{bmatrix}
\begin{bmatrix}
u_1 \\
v_1 \\
\vdots \\
u_{N} \\
v_{N}
\end{bmatrix}
```

上記のように、$2N×2N$の行列になります。ここで、三角形要素の要素剛性マトリクスは節点数3個なので$6×6$の行列になります。そのため、全体剛性マトリクスに足し合わせるために対応する行列の位置を求める必要があります。
三角形要素の節点番号を$i$, $j$, $k$として要素の変位ベクトルを$u_e$とすると、全体剛性マトリクスに格納する位置は下記のように表すことができます。

```math
K_e u_e = 
\begin{bmatrix}
K_{2i-1, 2i-1} && K_{2i-1, 2i} && K_{2i-1, 2j-1} && K_{2i-1, 2j} && K_{2i-1, 2k-1} && K_{2i-1, 2k} \\
K_{2i, 2i-1} && K_{2i, 2i} && K_{2i, 2j-1} && K_{2i, 2j} && K_{2i, 2k-1} && K_{2i, 2k} \\
K_{2j-1, 2i-1} && K_{2j-1, 2i} && K_{2j-1, 2j-1} && K_{2j-1, 2j} && K_{2j-1, 2k-1} && K_{2j-1, 2k} \\
K_{2j, 2i-1} && K_{2j, 2i} && K_{2j, 2j-1} && K_{2j, 2j} && K_{2j, 2k-1} && K_{2j, 2k} \\
K_{2k-1, 2i-1} && K_{2k-1, 2i} && K_{2k-1, 2j-1} && K_{2k-1, 2j} && K_{2k-1, 2k-1} && K_{2k-1, 2k} \\
K_{2k, 2i-1} && K_{2k, 2i} && K_{2k, 2j-1} && K_{2k, 2j} && K_{2k, 2k-1} && K_{2k, 2k} 
\end{bmatrix}
\begin{bmatrix}
u_i \\
v_i \\
u_j \\
v_j \\
u_k \\
v_k
\end{bmatrix}
```

今回の場合は下記のような計算になります。

要素1の要素剛性行列$K_e^1$

```math
K_e^1 u_e^1 = 
\begin{bmatrix}
K_{11}^1 && K_{12}^1 && K_{13}^1 && K_{14}^1 && K_{17}^1 && K_{18}^1 \\
K_{21}^1 && K_{22}^1 && K_{23}^1 && K_{24}^1 && K_{27}^1 && K_{28}^1 \\
K_{31}^1 && K_{32}^1 && K_{33}^1 && K_{34}^1 && K_{37}^1 && K_{38}^1 \\
K_{41}^1 && K_{42}^1 && K_{43}^1 && K_{44}^1 && K_{47}^1 && K_{48}^1 \\
K_{71}^1 && K_{72}^1 && K_{73}^1 && K_{74}^1 && K_{77}^1 && K_{78}^1 \\
K_{81}^1 && K_{82}^1 && K_{83}^1 && K_{84}^1 && K_{87}^1 && K_{88}^1 \\
\end{bmatrix}
\begin{bmatrix}
u_1 \\
v_1 \\
u_2 \\
v_2 \\
u_4 \\
v_4
\end{bmatrix}
```

要素2の要素剛性行列$K_e^2$

```math
K_e^2 u_e^2 = 
\begin{bmatrix}
K_{11}^2 && K_{12}^2 && K_{17}^2 && K_{18}^2 && K_{15}^2 && K_{16}^2 \\
K_{21}^2 && K_{22}^2 && K_{27}^2 && K_{28}^2 && K_{25}^2 && K_{26}^2 \\
K_{71}^2 && K_{72}^2 && K_{77}^2 && K_{78}^2 && K_{75}^2 && K_{76}^2 \\
K_{81}^2 && K_{82}^2 && K_{87}^2 && K_{88}^2 && K_{85}^2 && K_{86}^2 \\
K_{51}^2 && K_{52}^2 && K_{57}^2 && K_{58}^2 && K_{55}^2 && K_{56}^2 \\
K_{61}^2 && K_{62}^2 && K_{67}^2 && K_{68}^2 && K_{65}^2 && K_{66}^2 
\end{bmatrix}
\begin{bmatrix}
u_1 \\
v_1 \\
u_4 \\
v_4 \\
u_3 \\
v_3
\end{bmatrix}
```

全体剛性マトリクス

```math
K = K_e^1 + K_e^2 = \\
\begin{bmatrix}
K_{11}^1 + K_{11}^2 && K_{12}^1 + K_{12}^2 && K_{13}^1 && K_{14}^1 && K_{15}^2 && K_{16}^2 && K_{17}^1 + K_{17}^2 && K_{18}^1 + K_{18}^2 \\
K_{21}^1 + K_{21}^2 && K_{22}^1 + K_{22}^2 && K_{23}^1 && K_{24}^1 && K_{25}^2 && K_{26}^2 && K_{27}^1 + K_{27}^2 && K_{28}^1 + K_{28}^2 \\
K_{31}^1 && K_{32}^1 && K_{33}^1 && K_{34}^1 && 0 && 0 && K_{37}^1 && K_{38}^1 \\
K_{41}^1 && K_{42}^1 && K_{43}^1 && K_{44}^1 && 0 && 0 && K_{47}^1 && K_{48}^1 \\
K_{51}^2 && K_{52}^2 && 0 &&0 && K_{55}^2 && K_{56}^2 && K_{57}^2 && K_{58}^2\\
K_{61}^2 && K_{62}^2 && 0 && 0 && K_{65}^2 && K_{66}^2 &&K_{67}^2 && K_{68}^2 \\
K_{71}^1 + K_{71}^2 && K_{72}^1 + K_{71}^2 && K_{73}^1 && K_{74}^1 && K_{75}^2 && K_{76}^2 && K_{77}^1 + K_{77}^2 && K_{78}^1 + K_{78}^2 \\
K_{81}^1 + K_{81}^2 && K_{82}^1 + K_{82}^2 && K_{83}^1 && K_{84}^1 && K_{85}^2 && K_{86}^2 && K_{87}^1 + K_{87}^2 && K_{88}^1 + K_{88}^2 \\
\end{bmatrix}
```

### 単点拘束の境界条件の処理
次に$Ku=f$に単点拘束の境界条件を考慮した処理を行います。単点拘束とは一つの節点自由度で構成される拘束のことで、例えば$u_1 = 0$などのことを言います。
この例題では、節点1,2が完全拘束されているので、$u_1, v_1 = 0$、$u_3, v_3 = 0$となり、この条件を考慮した式に変形します。
ここで、簡単のため、剛性マトリクス$3×3$で既知量$u_2 = \bar{u}$の場合で考えます。
すると、下記のように表すことができます。

```math
\begin{bmatrix}
K_{11} && K_{12} && K_{13} \\
K_{21} && K_{22} && K_{23} \\
K_{31} && K_{32} && K_{33} \\
\end{bmatrix}
\begin{bmatrix}
u_1 \\
\bar{u} \\
u_3 \\
\end{bmatrix}
=
\begin{bmatrix}
f_1 \\
f_2 \\
f_3 \\
\end{bmatrix}
\\
K_{11} u_1 +  K_{12} \bar{u} + K_{13} u_3 = f_1 \\
K_{21} u_1 +  K_{22} \bar{u} + K_{23} u_3 = f_2 \\
K_{31} u_1 +  K_{32} \bar{u} + K_{33} u_3 = f_3 \\
```

既知量を右に移動すると

```math
K_{11} u_1 + K_{13} u_3 = f_1 - K_{12} \bar{u} \\
K_{21} u_1 + K_{23} u_3 = f_2 - K_{22} \bar{u} \\
K_{31} u_1 + K_{33} u_3 = f_3 - K_{32} \bar{u}
```

となり、真ん中の式を省いて代わりに、$u_2 = \bar{u}$を代入すると、

```math
K_{11} u_1 + K_{13} u_3 = f_1 - K_{12} \bar{u} \\
u_2 = \bar{u} \\
K_{31} u_1 + K_{33} u_3 = f_3 - K_{32} \bar{u}　\\
\begin{bmatrix}
K_{11} && 0 && K_{13} \\
0 && 1 && 0 \\
K_{31} && 0 && K_{33} 
\end{bmatrix}
\begin{bmatrix}
u_1 \\
\bar{u} \\
u_3 
\end{bmatrix}
=
\begin{bmatrix}
f_1 \\
\bar{u} \\
f_3
\end{bmatrix}
-
\begin{bmatrix}
K_{12}  \\
0 \\
K_{32}
\end{bmatrix}
\bar{u}
```

とすることができます。ここで、式を一つ省くことを行っていますが、一般に節点が拘束されている点の荷重値(反力)は未知数なので、未知の値$f_2$を行列から消して変位を求めることが可能となります。
この処理を例題に行うと、下記のようになります。

```math
Ku = f \\
\begin{bmatrix}
1 && 0 && 0 && 0　&& 0　&& 0 && 0　&&0 \\
0 && 1 && 0 && 0　&& 0　&& 0 && 0　&&0 \\
0 && 0 && K_{33} && K_{34}　&& 0　&& 0 && K_{37}　&& K_{38} \\
0 && 0 && K_{43} && K_{44}　&& 0　&& 0 && K_{47}　&& K_{48} \\
0 && 0 && 0 && 0　&& 1　&& 0 && 0　&& 0 \\
0 && 0 && 0 && 0　&& 0　&& 1 && 0　&& 0 \\
0 && 0 && K_{73} && K_{74}　&& 0　&& 0 && K_{77}　&& K_{78} \\
0 && 0 && K_{83} && K_{84}　&& 0　&& 0 && K_{87}　&& K_{88} 
\end{bmatrix}
\begin{bmatrix}
u_1 \\
v_1 \\
u_2 \\
v_2 \\
u_3 \\
v_3 \\
u_4 \\
v_4
\end{bmatrix}
=
\begin{bmatrix}
0 \\
0 \\
0 \\
0 \\
0 \\
0 \\
0 \\
1000
\end{bmatrix}
```

### 多点拘束の境界条件の処理
次に多点拘束の条件を考慮した剛性マトリクスに変形します。
この例題では$C$マトリクスは下記のようになります。

```math
\begin{bmatrix}
K && C^T \\
C && 0 
\end{bmatrix}
\begin{bmatrix}
u \\
\lambda
\end{bmatrix}
=
\begin{bmatrix}
F \\
d
\end{bmatrix}
\\
C = 
\begin{bmatrix}
0 && 0 && -sin45° && cos45° && 0 && 0 && 0 && 0
\end{bmatrix}
\\
d= 0
```

上記の式の逆行列をとることでようやく変位$u$を求めることができます。
上記の処理をPythonのクラスで実装してみます。

・Boundary2dクラス : 境界条件の格納用
・FEM2dクラス      : 有限要素法で変位の計算用クラス

Boundary2dクラスは境界条件を格納するためのクラスで、addSPC関数で単点拘束を追加、addMPC関数で多点拘束を追加します。addMPC関数はCマトリクス、dベクトルの値を行毎に入力していく形式で実装しています。また、荷重はaddForce関数で追加します。makeDispVector関数、makeForceVector関数とmakeMPCmatrixes関数はFEM2dクラス内で使うための関数で、設定した拘束条件を行列、ベクトルの形で返します。

境界条件の格納用クラス : Boundary2d.py

```python
import numpy as np

class Boundary2d:

    def __init__(self, nodeNum):
        self.nodeNum = nodeNum  # 全節点数
        self.dispNodeNo = []    # 単点拘束の節点番号
        self.dispX = []         # 単点拘束の強制変位x
        self.dispY = []         # 単点拘束の強制変位y
        self.forceNodeNo = []   # 荷重が作用する節点番号
        self.forceX = []        # x方向の荷重
        self.forceY = []        # y方向の荷重
        self.matC = np.empty((0, self.nodeNum * 2))   # 多点拘束用のCマトリクス
        self.vecd = np.empty(0)                       # 多点拘束用のdベクトル

    # 単点拘束を追加する
    def addSPC(self, nodeNo, x, y):

        self.dispNodeNo.append(nodeNo)
        self.dispX.append(x)
        self.dispY.append(y)

    # 多点拘束を追加する
    # 条件式 : vecC x u = d
    def addMPC(self, vecC, d):
        self.matC = np.vstack((self.matC, vecC))
        self.vecd = np.hstack((self.vecd, d))

    # 単点拘束条件から変位ベクトルを作成する
    def makeDispVector(self):

        vecd = np.array([None] * self.nodeNum * 2)
        for i in range(len(self.dispNodeNo)):
            vecd[(self.dispNodeNo[i] - 1) * 2] = self.dispX[i]
            vecd[(self.dispNodeNo[i] - 1) * 2 + 1] = self.dispY[i]
        
        return vecd
    
    # 荷重を追加する
    def addForce(self, nodeNo, fx, fy):
        
        self.forceNodeNo.append(nodeNo)
        self.forceX.append(fx)
        self.forceY.append(fy)
    
    # 境界条件から荷重ベクトルを作成する
    def makeForceVector(self):

        vecf = np.array(np.zeros([self.nodeNum * 2]))
        for i in range(len(self.forceNodeNo)):
            vecf[(self.forceNodeNo[i] - 1) * 2] += self.forceX[i]
            vecf[(self.forceNodeNo[i] - 1) * 2 + 1] += self.forceY[i]
        
        return vecf

    # 多点拘束の境界条件を表すCマトリクス、dベクトルを作成する
    def makeMPCmatrixes(self):
        
        return self.matC, self.vecd
```

FEM2dクラスは有限要素法で変位$u$を求めるためのクラスで、全節点をNode2dクラスのリストで、全要素をd2cps3クラスのリストで、境界条件をBoundary2dクラスの形式で入力値をとります。そして、analysis関数で初期化した情報から変位$u$を求めて返します。

変位の計算用クラス : FEM2d.py

```python
import numpy as np
import numpy.linalg as LA

class FEM2d:

    def __init__(self, nodes, elements, bound):
        self.nodes = nodes         # 節点は1から始まる順番で並んでいる前提(Node2d型のリスト)
        self.elements = elements   # 要素のリスト(d2cps型のリスト)
        self.bound = bound         # 境界条件(Boundary2d型)

    def analysis(self):
        # 境界条件を考慮しないKマトリクスを作成する
        matK = self.makeKmatrix()

        # 荷重ベクトルを作成する
        vecf = self.bound.makeForceVector()

        # 境界条件を考慮したKマトリクス、荷重ベクトルを作成する
        matKc, vecfc, mpcNum = self.setBoundCondition(matK, vecf)

        # 変位ベクトルを計算する
        vecTmp = LA.solve(matKc, vecfc)
        vecDisp = np.delete(vecTmp, slice(len(vecTmp) - mpcNum, len(vecTmp)))
        self.vecDisp = vecDisp

        return vecDisp

    # 境界条件を考慮しないKマトリクスを作成する
    def makeKmatrix(self):

        matK = np.matrix(np.zeros((len(self.nodes) * 2, len(self.nodes) * 2)))
        for elem in self.elements:
            # keマトリクスを計算する
            matKe = elem.makeKematrix()

            # Kマトリクスに代入する
            for c in range(len(elem.nodes) * 2):
                ct = (elem.nodes[c // 2].no - 1) * 2 + c % 2
                for r in range(len(elem.nodes) * 2):
                    rt = (elem.nodes[r // 2].no - 1) * 2 + r % 2
                    matK[ct, rt] += matKe[c, r]
        return matK

    # Kマトリクス、荷重ベクトルに境界条件を考慮する
    def setBoundCondition(self, matK, vecf):
        matKc = np.copy(matK)
        vecfc = np.copy(vecf)

        # 単点拘束条件を考慮したKマトリクス、荷重ベクトルを作成する
        vecDisp = self.bound.makeDispVector()
        for i in range(len(vecDisp)):
            if not vecDisp[i] == None:
                # Kマトリクスからi列を抽出する
                vecx = np.array(matK[:, i]).flatten()

                # 変位ベクトルi列の影響を荷重ベクトルに適用する
                vecfc = vecfc - vecDisp[i] * vecx

                # Kマトリクスのi行、i列を全て0にし、i行i列の値を1にする
                matKc[:, i] = 0.0
                matKc[i, :] = 0.0
                matKc[i, i] = 1.0
        for i in range(len(vecDisp)):
            if not vecDisp[i] == None:
                vecfc[i] = vecDisp[i]

        # 多節点拘束条件を考慮したKマトリクス、荷重ベクトルを作成する
        matC, vecd = self.bound.makeMPCmatrixes()
        mpcNum = len(vecd)   # 多節点拘束の条件式の数を求める
        if mpcNum > 0:
            matKc = np.hstack((matKc, matC.T))
            tmpMat = np.hstack((matC, np.zeros((matC.shape[0], matC.shape[0]))))
            matKc = np.vstack((matKc, tmpMat))
            vecfc = np.hstack((vecf, vecd))

        return matKc, vecfc, mpcNum
```

### メインの処理
上記で実装したPythonのクラスを使用して変位$u$を求める処理を実装します。
処理の流れは下記の通りになります。

・Node2dクラスを使って、全節点、要素の節点をそれぞれリスト形式で作成する。
・d2cps3クラスに要素番号、節点のリスト、厚さ、ヤング率、ポアソン比を与えて、全要素のリストを作成する。
・Boundary2dクラスに節点数を渡して初期化する。
・Boundary2dクラスに荷重、単点拘束、多点拘束の条件を与える。
・FEM2dクラスにNode2dクラスの全節点のリスト、d2cps3クラスの全要素のリスト、Boundary2dクラスを与えて初期化する。
・FEM2dクラスのalanaysis関数を呼び出し、変位を計算する


メインの処理 : main.py

```python
from Node2d import Node2d
from d2cps3 import d2cps3
from Boundary2d import Boundary2d
from FEM2d import FEM2d
import numpy as np

if __name__ == '__main__':
    # 節点のリストを作成する
    node1 = Node2d(1, 0.0, 0.0)
    node2 = Node2d(2, 1.0, 0.0)
    node3 = Node2d(3, 0.0, 1.0)
    node4 = Node2d(4, 1.0, 1.0)
    nodes1 = [node1, node2, node4]
    nodes2 = [node1, node4, node3]
    nodes = [node1, node2, node3, node4]

    # 要素のリストを作成する
    thickness = 1
    young = 210000
    poisson = 0.3
    elem1 = d2cps3(1, nodes1, thickness, young, poisson)
    elem2 = d2cps3(2, nodes2, thickness, young, poisson)
    elems = [elem1, elem2]

    # 境界条件を設定する
    theta = 45
    mpc1 = np.array([0, 0, -np.sin(np.deg2rad(theta)), np.cos(np.deg2rad(theta)), 0, 0, 0, 0])
    d1 = 0
    bound = Boundary2d(len(nodes))
    bound.addSPC(1, 0.0, 0.0)
    bound.addSPC(3, 0.0, 0.0)
    bound.addMPC(mpc1, d1)
    bound.addForce(4, 0.0, 1000.0)

    # 有限要素法で変位を計算する
    fem2d = FEM2d(nodes, elems, bound)
    vecDisp = fem2d.analysis()
    print("変位ベクトル : ", vecDisp)
```

### 計算結果を比較する
上記の計算結果が正しいかAbaqus2021 Studen Editionを使用して確かめてみます。

Abaqusの入力ファイル

```
*node
1, 0.0, 0.0
2, 1.0, 0.0
3, 0.0, 1.0
4, 1.0, 1.0
*nset, nset=mpcNode
2
*transform, nset=mpcNode
1.0, 1.0, 0.0, -1.0, 1.0, 0.0
*element, type=cps3, elset=Elems
1, 1, 2, 4
2, 1, 4, 3
*material, name=mat
*elastic
210000, 0.3
*solid section, elset=Elems, material=mat
1.0
*boundary
1, 1, 2
3, 1, 2
2, 2, 2
*step
*static
*cload
4, 2, 1000.0
*end step
```

結果

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/762468/2d78859c-695d-e6cf-1e63-31581546510b.png)

$10^{-3}$オーダーで一致することが確認できます。

使用したpythonのコードは下記にアップロードしています。
https://github.com/Altaka4128/FEM2d_MPC

# 参考
・[IFEM.Ch09.pdf](https://kis.tu.kielce.pl/mo/COLORADO_FEM/colorado/IFEM.Ch09.pdf)
・[Multi-Point Constraints](https://mashayekhi.iut.ac.ir/sites/mashayekhi.iut.ac.ir/files//files_course/lesson_16.pdf)