mechanics

mechanics

- Rigid body
- Constrained dynamics
- Position-based dynamics

mechanics-01-rigid-body

$$$
% From AsciiMath
\newcommand{\bb}[0]{\mathbf}
\newcommand{\bbb}[0]{\mathbb}
\newcommand{\RR}[0]{\bbb{R}}
\newcommand{\xx}[0]{\times}
\newcommand{\del}[0]{\partial}
% Quick begin/end
\newcommand{\Ba}[0]{\begin{aligned}}
\newcommand{\Ea}[0]{\end{aligned}}
\newcommand{\Bm}[0]{\begin{matrix}}
\newcommand{\Em}[0]{\end{matrix}}
\newcommand{\Bmp}[0]{\begin{pmatrix}}
\newcommand{\Emp}[0]{\end{pmatrix}}
% Others
\newcommand{\t}[0]{\text}
\renewcommand{\ll}[0]{\left}
\newcommand{\rr}[0]{\right}
\newcommand{\defiff}[0]{\overset{\text{def}}{\iff}}
\newcommand\ddfrac[2]{{\displaystyle\frac{\displaystyle #1}{\displaystyle #2}}}
$$$

# Rigid body theory

## Collision reaction

For two colliding rigid bodies, we write:

- $I_i$ : moment of inertia
- $m_i$ : mass
- $v_i, \hat{v}_i$ : velocity of center of mass (before and after the collision)
- $A_i$ : body frame (i.e. SO(3))
- $w_i, \hat{w}_i$ : angular velocity (in body frame) (before and after the collision)
- $r_i$ : collision point from center of mass (in world frame)
- $v_{r_i}, \hat{v}_{r_i}$ : velocity collision point (before and after the collision)
- $e$ : COR (coefficient of restitution)
- $n = n_{21} = - n_{12}$ : collision normal ("direction" from body 1 to body 2) (in world frame).

We define impulse $j_{21}$ from body 1 to body 2 by:

- $j_{21} \equiv  \int_{[0, \delta]} f_{21} \parallel n_{21}$,

and we write $j = j_{21} = - j_{12} = a n$ with $a \in R_{\ge 0}$.

Then, from Newton's 2nd law,

- $\hat{v} - v = \frac{j}{m}$.

From Euler's equation,

- $A I (\hat{w} - w) = r \times j$.

From "restituion" equation,

- $n \cdot (\hat{v}_{r_2} - \hat{v}_{r_1}) = - e n \cdot (v_{r_2} - v_{r_1})$.

Also, velocity of point on rigid body in world frame is:

- $v_r = v + A (w \times (A^{-1} r))$.

From these equations, we can solve variable $a$.

First, we see,

$$
\Ba
&n \cdot (\hat{v}_{r_2} - \hat{v}_{r_1}) = - e n \cdot (v_{r_2} - v_{r_1}) \\
  &\iff
        n \cdot (\hat{v}_{r_2} - v_{r_2}) - n \cdot (\hat{v}_{r_1} - v_{r_1})
            = - (1 + e) n \cdot(v_{r_2} - v_{r_1}).
\Ea
$$

Next, we see,

$$
\Ba
\hat{v}_r - v_r
    &= (\hat{v} - v) + A ((\hat{w} - w) \times (A^{-1} r)) \\
    &= \frac{j}{m} + A \ll( \ll( I^{-1} A^{-1} (r \times j) \rr) \times (A^{-1} r) \rr) \\
    &= \frac{j}{m} + \ll( A I^{-1} A^{-1} (r \times j) \rr) \times r.
\Ea
$$

So, especially:

$$
\Ba
\hat{v}_{r_2} - v_{r_2}
    &= a \ll( \frac{n}{m_2} + \ll( A_2 I_2^{-1} A_2^{-1} (r_2 \times n) \rr) \times r_2 \rr) \\
\hat{v}_{r_1} - v_{r_1}
    &= - a \ll( \frac{n}{m_1} + \ll( A_1 I_1^{-1} A_1^{-1} (r_1 \times n) \rr) \times r_1 \rr).
\Ea
$$

Therefore, we have:

$$
\Ba
&n \cdot (\hat{v}_{r_2} - v_{r_2}) - n \cdot (\hat{v}_{r_1} - v_{r_1}) \\
    &= 
         a \ll(
             \frac{1}{m_1} + \frac{1}{m_2} +
             n \cdot
             \big(
                 \ll( A_1 I_1^{-1} A_1^{-1} (r_1 \times n) \rr) \times r_1 +
                 \ll( A_2 I_2^{-1} A_2^{-1} (r_2 \times n) \rr) \times r_2
             \big)
           \rr),
\Ea
$$

and we obtain:

$$
\Ba
a =
    \ddfrac{
        - (1 + e) n \cdot(v_{r_2} - v_{r_1})
    }{
        \frac{1}{m_1} + \frac{1}{m_2} +
            n \cdot
                \big(
                    \ll( A_1 I_1^{-1} A_1^{-1} (r_1 \times n) \rr) \times r_1 +
                    \ll( A_2 I_2^{-1} A_2^{-1} (r_2 \times n) \rr) \times r_2
                \big).
    }
\Ea
$$


## Coulomb friction for collision reaction

TODO


## Contact force

TODO

mechanics-02-contrainted-dynamics

$$$
% From AsciiMath
\newcommand{\bb}[0]{\mathbf}
\newcommand{\bbb}[0]{\mathbb}
\newcommand{\RR}[0]{\bbb{R}}
\newcommand{\xx}[0]{\times}
\newcommand{\del}[0]{\partial}
% Quick begin/end
\newcommand{\Ba}[0]{\begin{aligned}}
\newcommand{\Ea}[0]{\end{aligned}}
\newcommand{\Bc}[0]{\begin{cases}}
\newcommand{\Ec}[0]{\end{cases}}
\newcommand{\Bm}[0]{\begin{matrix}}
\newcommand{\Em}[0]{\end{matrix}}
\newcommand{\Bmp}[0]{\begin{pmatrix}}
\newcommand{\Emp}[0]{\end{pmatrix}}
% Others
\newcommand{\t}[0]{\text}
\newcommand{\defiff}[0]{\overset{\text{def}}{\iff}}
\newcommand\ddfrac[2]{{\displaystyle\frac{\displaystyle #1}{\displaystyle #2}}}
$$$

## Constrained dynamics

***Notations***

- $G : \RR^n \to \RR^k$,
- $\bb{D}G_x \in \RR^{k \xx n}$ (we often omit argument $x$, i.e. $\bb{D}G \in \RR^{k \xx n}$),
- $x : \RR \to \RR^n : t \mapsto x_t$ (we often omit argument $t$,
    so we write e.g. $x \in \RR$, $\dot{x} \in \RR$, etc...),
- $M \in \RR^{n \xx n}$

### Prop. 1 (Sufficient condition for zero constraint work principle)

Given:

- $\del_t G(x_t) = \bb{0} \in \RR^k$,
- $\lambda \in \RR^k$,
- $\hat{f} = \bb{D}G^T \lambda \in \RR^n$,

then $\hat{f}^T \dot{x} = \bb{0}$.

N.B.

If we write $\hat{f}_i \equiv \bb{D}G_i^T \lambda_i \parallel \bb{D}G_i^T$,
then $\hat{f} = \sum_i \hat{f}_i$ and this represents each $\hat{f}_i$ comes from
corresponding constraint $G_i$.


***Proof***

We see that:

\[
\hat{f}^T \dot{x}
  = (\bb{D}G \lambda)^T \dot{x}
  = \lambda^T \bb{D}G \dot{x}
  = \lambda^T \del_t G(x_t) = 0.
\]


### Prop. 2 (Instantaneous constraint force formula)

Given:

- $\del_t^2 G(x_t) = \bb{0} \in \RR^{k}$,
- $\lambda \in \RR^k$,
- $\hat{f} = \bb{D}G^T \lambda \in \RR^n$,
- $M \ddot{x} = f + \hat{f}$,

then:

\[
\lambda = - (\bb{D}G M^{-1} \bb{D}G^T)^{-1} (\bb{D}G M^{-1} f + \bb{D}^2 G(\dot{x}, \dot{x})).
\]

***Proof***

Noting that
$~
\del_t^2 G(x_t) = \del_t (\bb{D}G \dot{x}) = \bb{D}G \ddot{x} + \bb{D}^2 G(\dot{x}, \dot{x})
$,
we see:

\[
\Ba
\bb{0}
  &= \bb{D}G \ddot{x} + \bb{D}^2 G(\dot{x}, \dot{x}) \\
  &= \bb{D}G M^{-1} (f + \hat{f}) + \bb{D}^2 G(\dot{x}, \dot{x}) \\
  &= \bb{D}G M^{-1} (f + \bb{D}G^T \lambda) + \bb{D}^2 G(\dot{x}, \dot{x}) \\
  &= \bb{D}G M^{-1} \bb{D}G^T \lambda + \bb{D}G M^{-1} f + \bb{D}^2 G(\dot{x}, \dot{x}).
\Ea
\]


### Prop. 3 (Inequality constraint is quadratic problem)

Given:

- $M \ddot{x} = f + \hat{f}$,
- $\del_t^2 G(x) \ge \bb{0} \in \RR^{k}$,
- $\bb{D}G_i \ne \bb{0} \in \RR^n$,
- $\lambda \ge \bb{0} \in \RR^k$,
- $\hat{f}_i = \bb{D}G_i^T \lambda_i \in \RR^n$,
- $\hat{f} = \sum_i \hat{f_i} = \bb{D}G^T \lambda \in \RR^n$,
- $
\forall i.~
  (\del_t^2 G_i(x) > 0 \implies \hat{f_i} = \bb{0}) \wedge 
  (\hat{f_i} \ne \bb{0} \implies \del_t^2 G_i(x) = 0)
$,

then:

- $A \lambda + b \ge \bb{0}$,
- $\lambda^T (A \lambda + b) = 0$,

where 

- $A = \bb{D}G M^{-1} \bb{D}G^T \in \RR^{k \xx k}$,
- $b = \bb{D}G M^{-1} f + \bb{D}^2 G(\dot{x}, \dot{x}) \in \RR^k$.

***Proof***

Since $\bb{D}G_i \ne \bb{0} \in \RR^n$, we have $\hat{f_i} = \bb{0} \iff \lambda_i = 0$,
and thus:

$$
\Ba
&(\del_t^2 G_i(x) > 0 \implies \hat{f_i} = \bb{0}) \wedge 
  (\hat{f_i} \ne \bb{0} \implies \del_t^2 G_i(x) = 0) \\
&\iff
  (\del_t^2 G_i(x) > 0 \implies \lambda_i = 0) \wedge 
    (\lambda_i \gt 0 \implies \del_t^2 G_i(x) = 0) \\
&\iff
  \lambda_i \del_t^2 G_i(x) = 0.
\Ea
$$

Therefore $\lambda^T \del_T^2 G(x) = 0$.

Now, by expanding $\del_T^2 G(x)$ as in **Prop.2**, we have
$\del_t^2 G(x) = A \lambda + b$ and it follows that:

$$
\Ba
&\lambda^T (A \lambda + b) = \lambda^T \del_T^2 G(x) =  0,\\
&A \lambda + b = \del_t^2 G(x) = A \lambda + b.
\Ea
$$

mechanics-03-position-base-dynamics

$$$
% From AsciiMath
\newcommand{\bb}[0]{\mathbf}
\newcommand{\bbb}[0]{\mathbb}
\newcommand{\RR}[0]{\bbb{R}}
\newcommand{\xx}[0]{\times}
\newcommand{\del}[0]{\partial}
% Quick begin/end
\newcommand{\Ba}[0]{\begin{aligned}}
\newcommand{\Ea}[0]{\end{aligned}}
\newcommand{\Bm}[0]{\begin{matrix}}
\newcommand{\Em}[0]{\end{matrix}}
\newcommand{\Bmp}[0]{\begin{pmatrix}}
\newcommand{\Emp}[0]{\end{pmatrix}}
% Others
\newcommand{\t}[0]{\text}
\newcommand{\defiff}[0]{\overset{\text{def}}{\iff}}
\newcommand\ddfrac[2]{{\displaystyle\frac{\displaystyle #1}{\displaystyle #2}}}
$$$

## Position based dynamics

***Notations***

- $x_i \in \RR^3$,
- $g(x_1, x_2, ...)$ where $g : \RR^{3n} \to R$,
- $\bb{D}g = \Bmp \del_{x_1} g,~ \del_{x_2} g,~ ... \Emp \in \RR^{1 \xx 3n}$,
- $M = \Bmp m_1 I & \dots \\ \vdots & \ddots \Emp \in R^{3n \xx 3n}$.

### Def. 1 

Given $g : R^{3n} \to R^k$, we define:

$\quad
\Ba
g : \t{translationally invariant}
  &\defiff \forall q \in \RR^3.
      ~~ g(x_1 + q, x_2 + q, ...) = g(x_1, x_2 , ...) \\
g : \t{rotationally invariant}
  &\defiff \forall A \in SO(3).
      ~~ g(A(x_1), A(x_2), ...) = g(x_1, x_2 , ...).
\Ea
$


### Prop. 2

$\quad
g : \t{translationally invariant}
\implies
  \sum_i \del_{x_i} g = \bb{0} \in \RR^{1 \xx 3}.
$

***Proof***

$\quad
\bb{0}
  = \del_q g(x_1, x_2, ...)
  = \del_q g(x_1 + q, x_2 + q, ...)
  = \bb{D}g|_{x_1, x_2, ...}
      \Bmp
        I \\ \vdots
      \Emp
  = \sum_i \del_{x_i} g.
$

### Prop. 3

$\quad
g : \t{rotationally invariant}
\implies
  \sum_i x_i \xx \del_{x_i} g^T = \bb{0} \in \RR^{3}
$

***Proof***

Taking $A_t = \exp(t \bb{u} \xx -)\in SO(3)$ (i.e. rotation of $t$-radian around $\bb{u}$-axis),
we have $\del_t A_t v = A_t (u \xx v)$ and $\del_t A_t|_{t = 0} v = u \xx v$. Thus,

$$
\Ba
R ~\ni~ \del_t g(A_t(x_1), A(x_2), ...)|_{t = 0}
  &= \bb{D}g \Bmp u \xx x_1 \\ u \xx x_2 \\ \vdots \Emp \\
  &= \sum_i \del_{x_i} g^T \cdot (u \xx x_i) \\
  &= u \cdot \sum_i x_i \xx \del_{x_i} g^T.
\Ea
$$

Here, $u \in \RR^3$ is arbitrary, so our result follows.


### Claim. 4

Given ODE

$$M \dot{v} = f = f_{\t{ext}} + f_{\t{int}},$$

its finite difference scheme produces:

$$ M (v^{(n+1)} - v^{(n)}) = h (f_{\t{ext}} + f_{\t{int}}).$$

By defining $\bar{v}^{(n)}$ s.t.

$$ M (\bar{v}^{(n)} - v^{(n)}) = h f_{\t{ext}}, $$

we have:

$$ M (v^{(n+1)} - \bar{v}^{(n)}) = h f_{\t{int}} \equiv \Delta p_{\t{int}}.$$

Further, we write $x^{(n)}, \bar{x}^{(n)}, x^{(n+1)}$ s.t.

$$
\Ba
& h v^{(n+1)} = x^{(n+1)} - x^{(n)}  \\
& h \bar{v}^{(n)} = \bar{x}^{(n)} - x^{(n)}
\Ea
$$

Then, we see:

$$
\bar{x}^{(n)}
  = x^{(n)} + h \bar{v}^{(n)}
  = x^{(n)} + h (v^{(n)} + h M^{-1} f_{\t{ext}}),
$$

and

$$
\Delta p_{\t{int}}
  = M (v^{(n+1)} - \bar{v}^{(n)})
  = M \frac{x^{(n+1)} - \bar{x}^{(n)}}{h}
  \equiv \frac{M}{h} \Delta x_{\t{int}}.
$$

Now, we postulate such $f_{\t{int}}$ comes from the constraint $g(x)$,
and we wish to find $x^{(n+1)}$ (or equivalently $\Delta x_{\t{int}}$) s.t.

$$
\Ba
&\bull~~
  0 = g(x^{(n+1)})
    \simeq g(\bar{x}^{(n)}) + \bb{D}g|_{\bar{x}^{(n)}} \cdot \Delta x_{\t{int}}, \\
&\bull~~
  \sum_i \Delta p_{i, \t{int}} = \bb{0},
    &\quad (\t{momentum conservation}) \\
&\bull~~  \sum_i x_i^{(n)} \xx \Delta p_{i, \t{int}} = \bb{0}.
    &\quad (\t{angular. momentum conservation})
\Ea
$$

For $g$ : translationally and rotationally invariant,
we know that, from Prop. 3 and Prop. 4, it holds that:

$$
\Ba
\Delta x_{\t{int}} ~\parallel M^{-1}\bb{D}g^T
  & \implies
    \Delta p_{\t{int}} \parallel M \Delta x_{\t{int}} \parallel \bb{D}g^T \\
  & \implies
    \Delta p_{i, \t{int}} \parallel \del_{x_i} g^T \\       
  & \implies
    \begin{cases}
      \sum_i \Delta p_{i, \t{int}} = \bb{0} \\
      \sum_i x_i^{(n)} \xx \Delta p_{i, \t{int}} = \bb{0}. \\
    \end{cases}
\Ea
$$

Here, taking $\Delta x_{\t{int}} = \lambda M^{-1} \bb{D}g^T$, we see:

$$
\Ba
& 0 = g(\bar{x}^{(n)}) + \bb{D}g|_{\bar{x}^{(n)}} \cdot \Delta x_{\t{int}}
    = g(\bar{x}^{(n)}) + \lambda \bb{D}g M^{-1} \bb{D}g \\
&\iff
    \lambda = - \frac{ g(\bar{x}^{(n)})}{\bb{D}g M^{-1} \bb{D}g }.
\Ea
$$