Constrained Dynamics

2011 Dec. written by 이주형 (Lee Ju Hyung)

1. 강체의 운동 방정식

n 개의 물체의 위치(\vec{X}) 와 방향(\vec{R}) 을 나타내는 벡터를 \vec{s}, 선속도(\vec{v}) 와 각속도(\vec{\omega})를 나타내는 벡터를 \vec{V}, 힘(\vec{F}) 과 토크(\vec{\tau})를 나타내는 벡터를 \vec{f}, 질량(m) 과 관성텐서(I) 를 나타내는 행렬을 M 이라고 하면

    \[\vec{s} = \begin{bmatrix} \vec{X_1} \\ \vec{R_1} \\ \vdots \\ \vec{X_n} \\ \vec{R_n} \end{bmatrix} ,\quad \vec{V} = \begin{bmatrix} \vec{v_1} \\ \vec{\omega_1} \\ \vdots \\ \vec{v_n} \\ \vec{\omega_n} \end{bmatrix} ,\quad \vec{f} = \begin{bmatrix} \vec{F_1} \\ \vec{\tau_1} \\ \vdots \\ \vec{F_n} \\ \vec{\tau_n} \end{bmatrix} ,\quad M = \begin{bmatrix} m_1 \mathbf{1_3} \\ & I_1 \\ & & \ddots \\ & & & m_n \mathbf{1_3} \\ & & & & I_n \end{bmatrix}\]

회전까지 고려하는 Newton 운동방정식은 아래와 같이 정리된다 1

    \[\begin{aligned} \dot{\vec{V}} &= M^{-1} \vec{f} \\ \dot{\vec{s}} &= S \vec{V} \\ \end{aligned}\]

여기서 S\vec{R_i} 이 quaternion \q_i} 일때 \vec{\omega_i} 를 quaternion 형태로 변환하는 행렬이다.

    \[S = \begin{bmatrix} \mathbf{1_3} \\ & Q_1 \\ & & \ddots \\ & & & \mathbf{1_3} \\ & & & & Q_n \end{bmatrix} ,\quad Q_i = \frac{1}{2} \begin{bmatrix} -q_{i\mathrm{x}} & -q_{i\mathrm{y}} & -q_{i\mathrm{z}} \\ q_{i\mathrm{w}} & q_{i\mathrm{z}} & -q_{i\mathrm{y}} \\ -q_{i\mathrm{z}} & q_{i\mathrm{w}} & q_{i\mathrm{x}} \\ q_{i\mathrm{y}} & -q_{i\mathrm{x}} & q_{i\mathrm{w}} \end{bmatrix}\]

여기서 Q_i\vec{\omega_i} 에 곱하기 위한 행렬이고 Q_i \vec{\omega_i}\vec{q_i} 의 시간 도함수 \dot{\vec{q_i}} = \frac{1}{2} \check{\omega_i} * \vec{q_i} 에서 \vec{\omega_i} 에 대한 쿼터니언 곱(*) 연산과 동치이다. 여기서 \check{\omega} 는 quaternion 화된 \vec{\omega}. (quaternion 시간 도함수의 자세한 유도과정은 Game Physics – David H. Eberly 참고)

    \[Q \vec{\omega} = \check{\omega} * \vec{q}\]

이제 앞에서 보았던 \vec{s}, \vec{V}, \vec{f}, M, S 등과 같이 앞으로 쓰일 물리량들은 대부분 블럭 행렬 형태이기 때문에 주의해서 보도록 하자.

2. Constraints

n 개의 물체에 대한 k 번째 (1 \le k \le s) 위치 구속 조건(constraint) 함수 C_k 는 다음과 같이 나타낼 수 있다.

    \[\begin{aligned} C_k &= C_k(\vec{s}(t))\\ &= C_k(\vec{X_1}(t), \vec{R_1}(t), \dots, \vec{X_n}(t), \vec{R_n}(t))\\ \end{aligned}\]

방정식 C_k = 0equality constraint 라고 하고, C_k \ge 0 같은 부등식 형태를 inequality constraint 라고 한다. 물리 시뮬레이션에서는 이와 같은 constraint 들을 만족시키기 위한 내부적인 force 나 impulse 를 구하는 것이 목표다.

2.1 Equality Constraints

위치를 구속하기 위해 C_k = 0 을 만족해야 한다고 가정하자. 다음 time step 에도 만족하려면 C_k 의 값이 커지거나 작아지면 안된다. 즉, 시간 도함수가 0 이어야 한다. C_kt 에 대해 미분하면 chain rule 에 의해,

    \[\begin{aligned} \dot{C_k} = \frac{ \partial C_k }{ \partial \vec{s} } \frac{ d \vec{s} }{ dt } = \frac{ \partial C_k }{ \partial \vec{s} } \dot{\vec{s}} = \frac{ \partial C_k }{ \partial \vec{s} } S \vec{V} = J_k \vec{V} = 0 \end{aligned}\]

모든 s 개의 constraint 들에 대해서 나타내면,

(1)   \[ \dot{C} = \begin{bmatrix}   J_{1} \\   \vdots \\   J_{s} \end{bmatrix} \vec{V} = \begin{bmatrix}   J_{11} & \dots & J_{1n} \\   \vdots & \ddots & \vdots \\   J_{s1} & \dots & J_{sn} \end{bmatrix}  \begin{bmatrix} \vec{v_1} \\ \vec{\omega_1} \\ \vdots \\ \vec{v_n} \\ \vec{\omega_n} \end{bmatrix} = J \vec{V} = \vec{0}\]

J 는 s x 6n 행렬이고 (2차원의 경우 s x 3n 행렬), \dot{C} 를 속도 constraint 함수라고 부른다 (C 는 위치 constraint 함수).

(1) 에서 J 의 행과 \vec{V} 는 서로 직교한다. constraint force 는 일(work) 을 하지 않으므로 마찬가지로 \vec{V} 와 직교해야 한다. 그러므로 J 의 행은 constraint force 와 평행하다.

(2)   \[\begin{aligned} J^T \vec{\lambda} &= \vec{f_c}  \end{aligned}\]

따라서 \vec{\lambda} 를 알면 constraint force 를 계산할 수 있다. (\vec{\lambda} 는 구속 조건하에 최소 작용힘의 원리를 나타내는 Lagrange multipliers 이다.) 이제 선형 운동 방정식으로부터 \vec{\lambda} 를 구하기 위한 식을 세워보자.

    \[\begin{aligned} \vec{V_+} &= \vec{V_-} + \dot{\vec{V}} \Delta t\\ &= \vec{V_-} + M^{-1} (\vec{f_c} + \vec{f_e}) \Delta t\\ &= \vec{V_-} + M^{-1} (J^T \vec{\lambda} + \vec{f_e}) \Delta t\\ \end{aligned}\]

여기서 \vec{V_-}, \vec{V_+}, \vec{f_c}, \vec{f_e} 는 각각 초기속도, \Delta t 만큼 지났을 때의 속도, 구속힘, 외부힘을 나타낸다. 이제 식 (1) 에 적용하면,

    \[\begin{aligned} \dot{C} &= J ( \vec{V_-} + M^{-1} J^T \vec{\lambda} \Delta t + M^{-1} \vec{f_e} \Delta t )\\ &= J \vec{V_-} + J M^{-1} J^T \vec{\lambda} \Delta t + J M^{-1} \vec{f_e} \Delta t\\ \end{aligned}\]

이항하면,

(3)   \[ \begin{aligned} J M^{-1} J^T \vec{\lambda} \Delta t &= \dot{C} - J (\vec{V_-} + M^{-1} \vec{f_e} \Delta t) \\ &= 0 - J (\vec{V_-} + M^{-1} \vec{f_e} \Delta t) \end{aligned}\]

여기서 \dot{C} 는 속도 constraint 함수를 만족하기 위해 0 이 되었다.

결국 \vec{\lambda} 를 구하는 것은 A\vec{x} = \vec{b} 형태의 선형 시스템 문제가 된다.

    \[\begin{aligned} A\vec{x} &= \vec{b} \end{aligned} ,\quad \begin{cases} \begin{aligned} A &= J M^{-1} J^T \\ \vec{x} &= \vec{\lambda} \Delta t \\ \vec{b} &= -J (\vec{V_-} + M^{-1} \vec{f_e} \Delta t) \end{aligned} \end{cases}\]

\vec{\lambda} 를 구하면 (2) 에 의해 constraint force 를 구할 수 있다.

2.2 Error Correction

위에서 구한 constraint force 는 속도 constraint 를 위반하지 않도록 내부적인 힘을 주지만, 이미 위치 constraint 를 위반하고 있는 상태를 해결하진 않는다.

    \[C + \dot{C} \Delta t = 0\]

    \[\dot{C} = -\frac{C}{\Delta t}\]

위식을 식 (3) 에 넣어서 위치 보정까지 고려한 constraint force 를 구할 수 있다. 한꺼번에 보정하면 떨림 현상이 발생할 수 있으므로 보정할 양 (bias) 을 조절한다. 식으로 나타내면,

    \[\dot{C} = -\beta \frac{C}{\Delta t}\]

2.3 Inequality Constraints

_inequality constraint_ 에서는 C \ge 0 을 만족해야 한다. 다시 말해,

    \[C + \dot{C} \Delta t \ge 0\]

    \[\dot{C} \ge -\frac{C}{\Delta t}\]

여기서 목표는 \dot{C} \ge - \beta \frac{C}{\Delta t} 를 만족시키는 최소 constraint force 를 구하는 것이다. 식 (3) 을 다시 정리하면,

    \[\begin{aligned} A x - b &= \dot{C} \\ A x - b &= w \ge \vec{0} \end{aligned} \quad,\quad \begin{cases} \begin{aligned} A &= J M^{-1} J^T \\ x &= \vec{\lambda} \Delta t \\ b &= -J (\vec{V_-} + M^{-1} \vec{f_e} \Delta t) - \beta \frac{C}{\Delta t} \end{aligned} \end{cases}\]

여기서 constraint force 는 constraint 를 만족하는 방향으로만 주어지기 때문에 \vec{x} \ge \vec{0} 이어야 한다. 또, \vec{w} \ge \vec{0} 이면 constraint 를 만족하기 때문에 constraint force 가 필요 없어지므로 \vec{x} = \vec{0} 이 되고, \vec{w} = \vec{0} 이면 constraint force 가 존재하거나 constraint force 없이 간신히 constraint 를 만족하는 것이 되므로 \vec{x} \ge \vec{0} 이 된다.

정리하면,

(4)   \[ A x - b = w\]

모든 i 에 대해서

(5)   \[ w_i \ge 0, x_i \ge 0\]

(6)   \[ w_i x_i = 0\]

이러한 구속조건하에 \vec{x} 를 구하는 문제를 LCP (Linear Complementarity Problem) 라고 한다2. 결국 LCP 식을 세워서 풀면 inequality constraint 를 만족하는 constraint force 를 구할 수 있다. 단, 항상 해가 존재하는 것은 아니다. 해는 존재하지 않을 수도 있고, 무수히 많은 해가 존재할 수도 있다. 하지만 행렬 A 가 positive definite 라면 유일해가 존재하고, 적어도 positive semi-definite 라면 최적해가 존재한다.

3. Complementarity Problems

3.1 MP (Mathematical Programming)

구속 조건 하에 목적함수 f 를 최적화시키는 것을 크게 MP (Mathematical Programming) 또는 Mathematical Optimization 라고 한다. 일반적인 목적은 g(x) \le 0 구속조건에서 f(x) 를 최소화시키는 해를 구하는 것이다. 여기서 목적 함수 f : \mathbb{R}^n \rightarrow \mathbb{R} 과, 제약 함수 g : \mathbb{R}^n \rightarrow \mathbb{R}^m 은 미분가능한 임의의 함수다. 해는 존재하지 않을 수도, 무수히 많을 수도 있지만 최적해를 가진다면 다음의 KKT 조건들을 만족해야 한다.

Stationarity:

    \[\nabla f(x^*) + \sum_{i=1}^m \lambda_i \nabla g_i(x^*) = 0\]

Primal feasibility:

    \[g_i(x^*) \le 0\]

Dual feasibility:

    \[\lambda_i \ge 0\]

Complementary slackness:

    \[\lambda_i g_i(x^*) = 0\]

KKT 조건은 최적해를 가지기 위한 필요 조건이다. 충분 조건이 더 있지만 일반적으로 fg 가 미분 가능하고, convex 함수라면 필요 충분 조건이 된다. KKT 조건에 대한 좀 더 자세한 내용은 KKT Optimality Conditions 문서를 참고하길 바란다.

3.2 LP (Linear Programming) 로부터 LCP 를 유도

LP (Linear Programming) 는 g(x) = (Ax - b, -x) \le 0 인 조건에서 f(x) = c^T x 를 최소화하는 것이다. 이 문제에 대한 KKT 조건들은 다음과 같다.

첫번째 조건은 다음과 같다.

    \[0 = \nabla f(x) + \lambda^T D g(x) = c^T + \lambda^T  \begin{bmatrix} A \\ -I \end{bmatrix}\]

여기서 In \times n 행렬이다. \lambda 를 두개의 블록 행렬 \lambda = (y, s_y) 로 나누면 최종 전개되었던 식은,

    \[s_y = c + A^T y\]

여기서 y 는 쌍대 문제의 정변수들이고 s_y 는 이완 변수들(slack variables) 이다.

두번째 조건은 g(x) \le 0 이며, 이는 x 의 실현 가능성 (primal feasibility) 을 나타내고, 세번째 조건은 \lambda \ge 0 은 쌍대 실현 가능성 (dual feasibility) 을 나타낸다.

그리고 마지막 조건은,

    \[0 = \lambda^T g(x) = (y, s_y)^T (Ax - b, -x)\]

블럭별로 나눠서 나타내면 y^T (Ax - b) = 0s_y^T x = 0 으로 상보 이완 (complementary slackness) 을 나타낸다.

이러한 조건들은 LP 를 LCP 로 재공식화 가능하게 해준다.

    \[w = Mz + q\]

    \[z^T w = 0, w \ge 0, z \ge 0\]

여기서,

    \[Ax - b + s_x = 0 ,\quad M =  \begin{bmatrix} 0 & A^T \\ -A & 0 \end{bmatrix} ,\quad z = \begin{bmatrix} x \\ y \end{bmatrix} ,\quad q =  \begin{bmatrix} c \\ b \end{bmatrix} ,\quad w = \begin{bmatrix} s_y \\ s_x \end{bmatrix}\]

3.3 Convex QP (Quadratic Programming) 로부터 LCP 를 유도

QP (Quadratic Programming) 는 LP 문제와 같은 구속조건 g(x) = (Ax - b, -x) \le 0 에서 목적함수가 2차식인 f(x) = x^T H x + c^T x + k 를 최소화하는 문제다. 행렬 H 가 positive (semi) definite 라면 f 는 convex 함수이고, 이때의 QP 를 Convex QP 로 분류한다.

QP 문제의 KKT 조건들은 바로 전의 LP 문제의 KKT 조건들과 첫번째 조건만 다르다.

    \[0 = \nabla f(x) + \lambda^T Dg(x) = 2x^T H + c^T + \lambda^T \begin{bmatrix} A \\ -I \end{bmatrix}\]

LP 문제처럼 \lambda = (y, s_y) 의 분해를 이용해서 최종식은,

    \[s_y = c + 2H x + A^T y\]

가 되며, KKT 조건들은 QP 를 LCP 로 재공식화 가능하게 해준다.

    \[w = Mz + q\]

    \[z^T w = 0, w \ge 0, z \ge 0\]

여기서,

    \[Ax - b + s_x = 0 ,\quad M =  \begin{bmatrix} 2H & A^T \\ -A & 0 \end{bmatrix} ,\quad z = \begin{bmatrix} x \\ y \end{bmatrix} ,\quad q =  \begin{bmatrix} c \\ b \end{bmatrix} ,\quad w = \begin{bmatrix} s_y \\ s_x \end{bmatrix}\]

앞서 LCP 에서 행렬 M 이 적어도 positive semi-definite 라면 최적해가 존재한다고 했다. 아래 식으로 M 의 positive definiteness 를 판단해 보면,

    \[z^T M z = z^T \begin{bmatrix} 2H & 0 \\ 0 & 0 \end{bmatrix} z +  z^T \begin{bmatrix} 0 & A^T \\ -A & 0 \end{bmatrix} z = z^T \begin{bmatrix} 2H & 0 \\ 0 & 0 \end{bmatrix} z\]

즉, H 가 적어도 positive semi-definite 이어야 한다는 걸 알 수 있다. 그러면 원래의 QP 의 목적함수 f 는 convex 함수다. 결과적으로 QP 중에 convex QP 만이 LCP 의 해를 구할 수 있는 문제가 된다.3

3.4 mLCP (Mixed Linear Complementarity Problem)

LCP 를 다시 정리해보면,

(7)   \[ y = Ax + b\]

    \[x^T y = 0, x \ge 0, y \ge 0\]

여기서,

    \[x, y, b \in \mathbb{R}^n\]

    \[A \in \mathbb{R}^{n \times n}\]

(7) 을 독립 변수 x 와 종속 변수 y 로 이루어진 affine 함수로 볼 수 있다. 두 변수 모두 non-negativity 조건을 만족하고, 서로 상보적일때 x 를 구하는 문제가 LCP 다. 이 LCP 에 선형 시스템을 끼워넣어 LCP 를 약간 일반화 시킬 수 있는데 이를 mLCP (Mixed Linear Complementarity Problem) 라고 부른다. 식으로 나타내면,

(8)   \[ \begin{bmatrix} y_1 \\ y_2 \end{bmatrix} = \begin{bmatrix} A_{11} & A_{12} \\ A_{21} & A_{22} \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} + \begin{bmatrix} b_1 \\ b_2 \end{bmatrix}\]

    \[y_1 = 0\]

    \[x_2^T y_2 = 0, x_2 \ge 0, y_2 \ge 0\]

여기서,

    \[x_1, y_1, b_1 \in \mathbb{R}^m\]

    \[x_2, y_2, b_2 \in \mathbb{R}^n\]

    \[\begin{aligned} A_{11} \in \mathbb{R}^{m \times m}, A_{12} \in \mathbb{R}^{m \times n} \\ A_{21} \in \mathbb{R}^{n \times m}, A_{22} \in \mathbb{R}^{n \times n} \end{aligned}\]

(8) 의 블록 행렬 A_{ij} 의 윗 부분이 선형 시스템 파트고, 아랫 부분이 LCP 파트다. A_{11} 이 non-singular 라면 x_1 = -A_{11}^{-1} (b_1 + A_{12} x_2) 가 되고, 이를 LCP 파트 식에 대입하면 선형 시스템이 없어지고 순수 LCP 가 된다. mLCP 는 극단적으로 n = 0 인 경우 A_{11} 만 남아서 선형 시스템이 될 수도 있고, m = 0 인 경우 A_{22} 만 남아서 일반 LCP 가 될 수도 있다.

참고로 LCP 의 affine 함수를 일반적인 함수로 정의한 것을 NCP (Nonlinear Complementarity Problem) 라고 부른다.

3.5 bLCP (Box-constrained LCP)

지금까지 LCP 에서 독립 변수 x_i 는 non-negativity 제한이 있었고, mLCP 에서는 일부 x_i 가 제한이 없기도 했다. bLCP (Box-constrained LCP) 는 이러한 제한을 사각 영역으로 제한함으로써 mLCP 를 좀 더 일반화시킨다.

    \[y = A x - b\]

모든 i 에 대해서

    \[x_i^\mathrm{low} \le x_i \le x_i^\mathrm{high}\]

이고,

    \[\begin{aligned} x_i = x_i^\mathrm{low} &\longrightarrow y_i \ge 0 \\ x_i^\mathrm{low} < x_i < x_i^\mathrm{high} &\longrightarrow y_i = 0 \\ x_i = x_i^\mathrm{high} &\longrightarrow y_i \le 0  \end{aligned}\]

bLCP

  • 모든 i 에 대해서 x_i^\mathrm{high} = +\infty, x_i^\mathrm{low} = -\infty 인 경우 항상 y_i = 0 이므로 선형 시스템이 된다.
  • 모든 i 에 대해서 x_i^\mathrm{high} = +\infty, x_i^\mathrm{low} = 0, 인 경우 LCP 와 같아진다.
  • 모든 i 에 대해서 x_i^\mathrm{high} = +\infty 이고 일부 i 에 대해서 x_i^\mathrm{low} = 0 이거나 x_i^\mathrm{low} = -\infty 인 경우 mLCP 와 같아진다.

참고로 bLCP 의 affine 함수를 일반적인 함수로 정의한 것을 MCP (Mixed Complementarity Problem) 라고 부른다.

4. Iterative Solver for Linear System

이번에는 선형 시스템의 근사적인 해를 구하는 방법을 설명하겠다. Ax = b 의 해가 존재할 때 가우스 소거법이나 역행렬을 이용해서 푸는 것을 직접법(direct methods) 이라고 하고, 해에 수렴하도록 예상값을 반복 적용해서 푸는 것을 반복법(iterative methods) 라고 한다.

행렬 A 를 simple 행렬 S 와 그 나머지 T 로 분리해서 식을 쓰면,

(9)   \[ Sx = b - Tx\]

이 식을 이용해서 iterative 하게 풀 수 있다. 예상값 x^k 는 더 근접한 예상값 x^{k+1} 을 산출한다.

(10)   \[ S x^{k+1} = b - T x^k\]

반복법의 목적은 소거 연산보다 되도록 빨리 근사해를 찾는 것이다. 진짜해 x 에 수렴할 때까지 무한정 반복해서 풀 수 있으나, 실용적으로 반복을 끝내는 조건은 아래 3 가지로 나눠 볼 수 있다.

* 최대 iteration count 에 도달하면 그만둔다.

* 오차값 \| b - A x^k \| 가 일정값 이하일 때 그만둔다.

* 수렴속도 \| x^{k+1} - x^k \| 가 일정값 이하일 때 그만둔다.

(9) 에서 식 (10) 을 빼면 오차 e^k 와 다음 step 오차 e^{k+1} 을 구할 수 있다.

    \[\begin{aligned} S e^{k+1} &= T e^k \\ e^{k+1} &= S^{-1} T e^k \end{aligned}\]

즉, S^{-1} Tspectral radius (eigenvalue 들 중 최대 절대값) 가 얼마나 빨리 수렴하는가를 결정한다.

S 의 성분을 구성하는 방법에 따라 여러가지 반복법들이 있다.

Jacobi method 의 경우 S 는 대각성분만으로 이루어져있고, GS method 의 경우에는 대각성분을 포함한 하삼각성분으로 되어있다. 아래에 나오는 A_DA 의 대각 성분으로 이루어진 행렬을, A_LA 의 대각 성분을 뺀 하삼각 행렬을, A_UA 의 대각 성분을 뺀 상삼각 행렬을 나타낸다.

    \[A = A_L + A_U + A_D\]

4.1 Jacobi Method

    \[\begin{aligned} S x^{k+1} &= b - T x^k \\ A_D x^{k+1} &= b - (A-A_D) x^k \\ x^{k+1} &= A_D^{-1} \left( b - \left( A-A_D \right) x^k \right) \\ x^{k+1} - x^k &= A_D^{-1} \left( b - A x^k \right) \end{aligned}\]

벡터 성분 별로 식을 정리하면,

    \[x^{k+1}_i - x^k_i = \Delta x_i = \frac{1}{A_{ii}} \left( b_i - A_i \cdot x^k \right)\]

전체 알고리즘의 의사 코드는 다음과 같다.

4.2 Gauss-Seidel Method

    \[\begin{aligned} S x^{k+1} &= b - T x^k \\ (A_D + A_L) x^{k+1} &= b - (A - A_D - A_L) x^k \\ A_D x^{k+1} &= b - A x^k + A_D x^k + A_L x^k - A_L x^{k+1} \\ x^{k+1} &= A_D^{-1} ( b - A x^k + A_D x^k + A_L x^k - A_L x^{k+1} ) \\ x^{k+1} &= A_D^{-1} ( b - (A - A_L) x^k - A_L x^{k+1} ) + x^k \\ x^{k+1} - x^k &= A_D^{-1} ( b - (A_D + A_U) x^k - A_L x^{k+1} ) \end{aligned}\]

벡터 성분 별로 식을 정리하면,

    \[x^{k+1}_i - x^k_i = \Delta x_i = \frac{1}{A_{ii}} \left( b_i - A_i \cdot x^k \right)\]

결과적으로 Jacobi method 와 수식은 같지만 x^{k+1}_i 가 다음 성분 계산에 바로 투입된다는 것이 다르다. 전체 알고리즘의 의사 코드는 다음과 같다.

4.3 SOR(Successive over-relaxation) Method

    \[\begin{aligned} \omega A x &= \omega b \\ (S - T) x &= \omega b \end{aligned}\]

여기서 최적의 \omegaS^{-1} Tspectral radius 가 최소가 되도록 하는 값이다.

    \[\begin{aligned} S x^{k+1} &= \omega b - T x^k \\ (A_D + \omega A_L) x^{k+1} &= \omega b - (\omega A - A_D - \omega A_L) x^k \\ x^{k+1} &= A_D^{-1} ( \omega b - (\omega A - A_D - \omega A_L) x^k - \omega A_L x^{k+1} ) \\ x^{k+1} &= \omega A_D^{-1} ( b - A x^k + A_L x^k - A_L x^{k+1} ) + x^k \\ x^{k+1} - x^k &= \omega A_D^{-1} ( b - (A_D + A_U) x^k - A_L x^{k+1} ) \end{aligned}\]

벡터 성분 별로 식을 정리하면,

    \[x^{k+1}_i - x^k_i = \Delta x_i = \frac{\omega}{A_{ii}} \left( b_i - A_i \cdot x^k \right)\]

\omega = 1 이면 GS method 와 동일하다. 전체 알고리즘의 의사 코드는 다음과 같다.

5. mLCP Solver

물리 엔진에서는 mLCP 를 이용해서 equality constraintinequality constraint 를 한꺼번에 풀 수가 있다. mLCP solver 는 여러가지 방법들이 개발되었으나 여기서는 비교적 간단하고 수렴속도가 빠른 반복법인 PGS (Projected Gauss-Seidel) method 만을 설명하겠다.

5.1 PGS (Projected Gauss-Seidel) Method

다음과 같은 mLCP 가 있을 때,

    \[\begin{aligned} Au + Cv + a &= 0 \\ C^T u + Bv + b &\ge 0 \\ v^T (C^T u + Bv + b) &= 0 \\ v &\ge 0 \end{aligned}\]

GS method 처럼 행렬 AB 를 쪼갠다.

(11)   \[ (A_L + A_D) u^{k+1} + A_U u^k + C v^k + a = 0\]

(12)   \[ C^T u^{k+1} + (B_L + B_D) v^{k+1} + B_U v^k + b \ge 0\]

(13)   \[ v^T (C^T u^{k+1} + (B_L + B_D) v^{k+1} + B_U v^k + b) = 0\]

(14)   \[ v \ge 0\]

(11) 을 정리하면 다음과 같다.

    \[u^{k+1} - u^k = -A_D^{-1}(A_D u^k + A_U u^k + A_L u^{k+1} + C v^k + a)\]

벡터 성분 별로 식을 정리하면,

    \[u^{k+1}_i - u^k_i = \Delta u_i = - \frac{1}{A_{ii}} \left( a_i + A_i \cdot x^k + C_i \cdot v^k \right)\]

u^{k+1} 을 구했으므로 이제 v^{k+1} 를 구한다. 부등식 (12) 가 등식이라고 가정하면,

    \[v^{k+1} - v^k = -B_D^{-1}(B_D v^k + B_U v^k + B_L v^{k+1} + C^T u^{k+1} + b)\]

벡터 성분 별로 식을 정리하면,

    \[v^{k+1}_i - v^k_i = \Delta v_i = - \frac{1}{B_{ii}} \left( b_i + B_i \cdot v^k + C^T_i \cdot u^{k+1} \right)\]

이 식은 성분 별 (12), (13) 을 만족한다. 여기에 동시에 (14) 까지 만족시키기 위해서,

    \[v^{k+1}_i = \mathrm{max} ( 0, \Delta v_i + v^k_i )\]

전체 알고리즘의 의사 코드는 다음과 같다.

약간 변형시켜 bLCP solver 를 만들 수 있다. 아래와 같다고 할때,

    \[M = \begin{bmatrix} A & C \\ C^T & B \end{bmatrix} ,\quad q = \begin{bmatrix} a \\ b \end{bmatrix}\]

bLCP solver 의 알고리즘 의사 코드는 다음과 같다.

6. References

  1. A Unified Framework for Rigid Body Dynamics
    Helmut Garstenauer
    http://www.digitalrune.com/Portals/0/Documents/A%20Unified%20Framework%20for%20Rigid%20Body%20Dynamics.pdf
  2. Iterative Dynamics with Temporal Coherence
    Erin Catto
    http://www.bulletphysics.com/ftp/pub/test/physics/papers/IterativeDynamics.pdf
  3. Game Physics 2nd Edition
    David H. Eberly
    http://www.amazon.com/Game-Physics-Second-David-Eberly/dp/0123749034
  4. Iterative Rigid Multibody Dynamics
    Tobias Preclik
    http://www10.informatik.uni-erlangen.de/Publications/Theses/2008/Preclik_DA08.pdf
  5. An Algorithm for the Fast Solution of Symmetric Linear Complementarity Problems
    José Luis Morales, Jorge Nocedal, Mikhail Smelyanskiy
    http://dl.acm.org/citation.cfm?id=1473180
  6. Introduction to Linear Algebra 4th Edition
    Gilbert Strang
    http://math.mit.edu/linearalgebra/
  1. 코리올리의 힘을 포함시켜야 하나 실제 시뮬레이션 중에는 항이 작기 때문에 무시해도 큰 지장은 없다.
  2. (6) 을 complementarity conditions 라고 하고, LCP (Linear Complementarity Problem) 라는 이름은 여기서 유래된다.
  3. QP 문제에서 이차항의 계수행렬인 H0 으로 놓음으로써 LP 문제와 동일해진다는 걸 알 수 있다.

Leave a Reply

Your email address will not be published. Required fields are marked *