May 10, 2012

0.1 Spatial Inertia Matrices

Given a rigid body with mass $m$ , center of mass ${\bf c}$ (with respect to body coordinates), and rotational inertia ${\bf J}$ (with respect to the center of mass), then the $6\times 6$ spatial inertia matrix is given by

${\bf M}=\left(\begin{matrix}m{\bf I}&-m[{\bf c}]\\ m[{\bf c}]&{\bf J}-m[{\bf c}][{\bf c}]\end{matrix}\right)$

where $[{\bf c}]$ denotes the $3\times 3$ skew-symmetric cross product matrix of ${\bf c}$ . The inverse is given by

${\bf M}^{{-1}}=\left(\begin{matrix}\frac{1}{m}{\bf I}-[{\bf c}]{\bf J}^{{-1}}[{\bf c}]&[{\bf c}]{\bf J}^{{-1}}\\ -{\bf J}^{{-1}}[{\bf c}]&{\bf J}^{{-1}}\end{matrix}\right)$

Note that this factors into ${\bf M}={\bf G}{\bf G}^{T}$ , with

${\bf G}=\left(\begin{matrix}\sqrt{m}{\bf I}&0\\ \sqrt{m}[{\bf c}]&{\bf L}\end{matrix}\right)$

and ${\bf J}={\bf L}{\bf L}^{T}$ .

0.2 Quaternions

A rotation consisting of a rotation $\theta$ about an axis ${\bf u}$ is described by a unit quarternion ${\bf q}$ according to

${\bf q}=(\cos(\theta/2),\sin(\theta/2){\bf u}).$

Given two quaterions ${\bf q}$ and ${\bf p}$ , with scalar components $q_{s}$ and $p_{s}$ and vector components ${\bf q}_{v}$ and ${\bf p}_{v}$ , the product ${\bf q}{\bf p}$ is defined by

${\bf q}{\bf p}=(q_{s}p_{s}-{\bf q}_{v}\cdot{\bf p}_{v},q_{s}{\bf p}_{v}+p_{s}{\bf q}_{v}+{\bf q}_{v}\times{\bf p}_{v}).$

Given an angular velocity $\boldsymbol{\omega}$ in base coordinates, the time derivative of a unit quaternion ${\bf q}$ is given by

$\dot{\bf q}=\frac{1}{2}(0,\boldsymbol{\omega}){\bf q}=\frac{1}{2}(-\sin(\theta/2)\boldsymbol{\omega}\cdot{\bf u},\cos(\theta/2)\boldsymbol{\omega}+\sin(\theta/2)\boldsymbol{\omega}\times{\bf u})$

(0.1)

with $(0,\boldsymbol{\omega})$ denoting a quaternion with zero scalar component and a vector component of $\boldsymbol{\omega}$ . If the angular velocity is instead given in the rotated coordinate system, then

$\dot{\bf q}=\frac{1}{2}{\bf q}(0,\boldsymbol{\omega})=\frac{1}{2}(-\sin(\theta/2)\boldsymbol{\omega}\cdot{\bf u},\cos(\theta/2)\boldsymbol{\omega}-\sin(\theta/2)\boldsymbol{\omega}\times{\bf u}).$

If a quaternion represents a rotated coordinate system, then the transformation of a vector ${\bf v}$ from rotated to world coordinates is given by

${}^{W}{\bf v}={\bf q}(0,{}^{R}{\bf v}){\bf q}^{{-1}}.$

0.3 Equivalence between LCP and Quadratic Programming

A quadratic program which calls for us to minimize

	$\displaystyle f(x)=\frac{1}{2}{\bf x}^{T}{\bf Q}{\bf x}+{\bf c}^{T}{\bf x},$
	$\displaystyle{\bf A}{\bf x}\geq{\bf b}$

with ${\bf Q}$ SPD, is equivalent to the LCP

	$\displaystyle{\bf M}{\bf z}+{\bf q}={\bf w},$
	$\displaystyle 0\leq{\bf z}\perp{\bf w}\geq 0$

where

	$\displaystyle{\bf M}$	$\displaystyle={\bf A}{\bf Q}^{{-1}}{\bf A}^{T}$
	$\displaystyle{\bf q}$	$\displaystyle=-{\bf b}-{\bf A}{\bf Q}^{{-1}}{\bf c}$
	$\displaystyle{\bf x}$	$\displaystyle={\bf Q}^{{-1}}({\bf A}^{T}{\bf z}-{\bf c}).$

0.4 Derivatives of roll, pitch and yaw angles

Spherical joints in ArtiSynth can be controlled using roll-pitch-yaw angles $\theta$ , $\phi$ , $\psi$ , in which an orientation is obtained by a rotation of $\theta$ about the $z$ axis, followed by a rotation of $\phi$ about the new $y^{{\prime}}$ axis, followed by a rotation of $\psi$ about the final $x^{{\prime\prime}}$ axis.

The total angular velocity $\boldsymbol{\omega}$ can be expressed in terms of the $\dot{\theta}$ , $\dot{\phi}$ and $\dot{\psi}$ according to

$\boldsymbol{\omega}={\bf z}\dot{\theta}+{\bf y}^{{\prime}}\dot{\phi}+{\bf x}^{{\prime\prime}}\dot{\psi}.$

Representing everything in terms of the orginal base frame, this leads to

$\left(\begin{matrix}\omega _{x}\\ \omega _{y}\\ \omega _{z}\end{matrix}\right)\;=\;\left(\begin{matrix}0&-s_{{\theta}}&c_{{\theta}}c_{{\phi}}\\ 0&c_{{\theta}}&s_{{\theta}}c_{{\phi}}\\ 1&0&-s_{{\phi}}\end{matrix}\right)\;\left(\begin{matrix}\dot{\theta}\\ \dot{\phi}\\ \dot{\psi}\end{matrix}\right)$

where $c_{{\theta}}$ , $s_{{\theta}}$ , $c_{{\phi}}$ , and $s_{{\phi}}$ are the cosines and sines of $\theta$ and $\phi$ . Inverting this relationship, we obtain

$\left(\begin{matrix}\dot{\theta}\\ \dot{\phi}\\ \dot{\psi}\end{matrix}\right)\;=\;\left(\begin{matrix}\frac{s_{{\phi}}c_{{\theta}}}{c_{{\phi}}}&\frac{s_{{\phi}}s_{{\theta}}}{c_{{\phi}}}&1\\ -s_{{\theta}}&c_{{\theta}}&0\\ \frac{c_{{\theta}}}{c_{{\phi}}}&\frac{s_{{\theta}}}{c_{{\phi}}}&0\end{matrix}\right)\;\left(\begin{matrix}\omega _{x}\\ \omega _{y}\\ \omega _{z}\end{matrix}\right)$

(0.2)

Note that there is a singularity when $c_{{\phi}}=0$ , since this implies alignment of the $z$ and $x^{{\prime\prime}}$ axes.

When implementing joint limit constraints for the $\theta$ , $\phi$ and $\psi$ angles, the corresponding constraint wrenches are formed by the rows of (0.2). The derivatives of these rows are given by

$\left(\begin{matrix}-s_{{\theta}}t_{{\phi}}\,\dot{\theta}+(1+t_{{\phi}}^{2})c_{{\theta}}\,\dot{\phi}&c_{{\theta}}t_{{\phi}}\,\dot{\theta}+(1+t_{{\phi}}^{2})s_{{\theta}}\,\dot{\phi}&0\\ -c_{{\theta}}\,\dot{\theta}&-s_{{\theta}}\,\dot{\theta}&0\\ (-s_{{\theta}}\,\dot{\theta}+c_{{\theta}}t_{{\phi}}\,\dot{\phi})/c_{{\phi}}&(c_{{\theta}}\,\dot{\theta}+s_{{\theta}}t_{{\phi}}\,\dot{\phi})/c_{{\phi}}&0\end{matrix}\right)$

where $t_{{\phi}}=\tan(\phi)$ .

0.5 Constraint Satisfaction as Projection

Assume we have a mechanical system with a mass matrix ${\bf M}$ , a state vector ${\bf x}$ , applied forces ${\bf f}_{x}$ , and a matrix of contraints ${\bf G}$ such that

${\bf G}\dot{\bf x}=0\quad\text{and}\quad{\bf G}\ddot{\bf x}=0.$

The corresponding constraint forces then lie in the range of ${\bf G}^{T}$ , and are computed such that

${\bf G}\ddot{\bf x}={\bf G}{\bf M}^{{-1}}({\bf f}_{x}+{\bf G}^{T}{\bf\lambda})=0.$

This implies that

${\bf\lambda}=-({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}{\bf G}{\bf M}^{{-1}}{\bf f}_{x}$

and so the constrained force ${\bf f}$ is given by

${\bf f}={\bf f}_{x}-{\bf G}^{T}({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}{\bf G}{\bf M}^{{-1}}{\bf f}_{x}$

${\bf f}={\bf P}_{f}{\bf f}_{x}$

where

${\bf P}_{f}\equiv({\bf I}-{\bf G}^{T}({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}{\bf G}{\bf M}^{{-1}})$

Similarly, we can ensure that velocity constraints are satisfied by applying an impulse ${\bf M}^{{-1}}{\bf G}^{T}{\bf\lambda}$ , so that the constrained velocity $\dot{\bf x}^{{\prime}}$ is given by

$\dot{\bf x}^{{\prime}}=\dot{\bf x}-{\bf M}^{{-1}}{\bf G}^{T}({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}{\bf G}\dot{\bf x}$

$\dot{\bf x}^{{\prime}}={\bf P}_{v}\dot{\bf x}$

where

${\bf P}_{v}\equiv({\bf I}-{\bf M}^{{-1}}{\bf G}^{T}({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}{\bf G}).$

It is easy to show that

${\bf P}_{v}={\bf P}_{f}^{T}.$

When constraints are expressed in the form of a KKT system, we can also show that

$\left(\begin{matrix}{\bf M}&-{\bf G}^{T}\\ {\bf G}&0\end{matrix}\right)^{{-1}}\;=\;\left(\begin{matrix}{\bf M}^{{-1}}{\bf P}_{f}&{\bf M}^{{-1}}{\bf G}^{T}({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}\\ -({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}{\bf G}{\bf M}^{{-1}}&({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}\end{matrix}\right)$

0.6 Solution of a general KKT system

Given a KKT system

$\left(\begin{matrix}{\bf M}&-{\bf G}^{T}\\ {\bf G}&0\end{matrix}\right)\left(\begin{matrix}{\bf v}\\ \boldsymbol{\lambda}\end{matrix}\right)\;=\;\left(\begin{matrix}{\bf b}\\ {\bf g}\end{matrix}\right)$

(0.3)

we have that

	$\displaystyle\boldsymbol{\lambda}=({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}({\bf g}-{\bf G}{\bf M}^{{-1}}{\bf b}),$
	$\displaystyle{\bf v}={\bf M}^{{-1}}{\bf b}+{\bf M}^{{-1}}{\bf G}^{T}({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}({\bf g}-{\bf G}{\bf M}^{{-1}}{\bf b})$

In terms of the projection matrices of the previous section, we have

${\bf v}={\bf M}^{{-1}}({\bf P}_{f}{\bf b}+{\bf G}^{T}({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}{\bf g})$

0.7 Turning a mixed LCP into a regular LCP

Assume the following mixed LCP:

	$\displaystyle{\bf M}{\bf x}-{\bf G}^{T}\boldsymbol{\lambda}-{\bf N}^{T}{\bf z}={\bf f},$
	$\displaystyle{\bf G}{\bf x}={\bf g},$
	$\displaystyle{\bf N}{\bf x}\geq{\bf n}.$

We can turn this into a pure LCP by solving for $\boldsymbol{\lambda}$

$\boldsymbol{\lambda}=-\left({\bf G}{\bf M}^{{-1}}{\bf G}^{T}\right)^{{-1}}{\bf G}{\bf M}^{{-1}}{\bf N}^{T}{\bf z}+\boldsymbol{\lambda}_{0},\quad\boldsymbol{\lambda}_{0}\equiv\left({\bf G}{\bf M}^{{-1}}{\bf G}^{T}\right)^{{-1}}\left({\bf g}-{\bf G}{\bf M}^{{-1}}{\bf f}\right),$

and then subsituting this into ${\bf N}{\bf x}\geq{\bf n}$ :

		$\displaystyle{\bf x}={\bf M}^{{-1}}{\bf f}+{\bf M}^{{-1}}G^{T}\boldsymbol{\lambda}+{\bf M}^{{-1}}{\bf N}^{T}{\bf z}$
	$\displaystyle\Rightarrow$	$\displaystyle{\bf N}{\bf M}^{{-1}}{\bf f}+{\bf N}{\bf M}^{{-1}}{\bf G}^{T}\boldsymbol{\lambda}+{\bf N}{\bf M}^{{-1}}{\bf N}^{T}{\bf z}\geq{\bf n}$
	$\displaystyle\Rightarrow$	$\displaystyle{\bf N}{\bf M}^{{-1}}{\bf f}+{\bf N}{\bf M}^{{-1}}{\bf G}^{T}\left[-\left({\bf G}{\bf M}^{{-1}}{\bf G}^{T}\right)^{{-1}}{\bf G}{\bf M}^{{-1}}{\bf N}^{T}{\bf z}+\boldsymbol{\lambda}_{0}\right]+{\bf N}{\bf M}^{{-1}}{\bf N}^{T}{\bf z}\geq{\bf n}$
	$\displaystyle\Rightarrow$	$\displaystyle{\bf N}{\bf M}^{{-1}}\left[{\bf I}-{\bf G}^{T}\left({\bf G}{\bf M}^{{-1}}{\bf G}^{T}\right)^{{-1}}{\bf G}{\bf M}^{{-1}}\right]{\bf N}^{T}{\bf z}+{\bf N}{\bf M}^{{-1}}{\bf f}+{\bf N}{\bf M}^{{-1}}{\bf G}^{T}\boldsymbol{\lambda}_{0}\geq{\bf n}$

In terms of the projection ${\bf P}_{f}$ described in the previous section, this can be expressed as

${\bf N}{\bf M}^{{-1}}{\bf P}_{f}{\bf N}^{T}{\bf z}+{\bf N}{\bf M}^{{-1}}{\bf f}+{\bf N}{\bf M}^{{-1}}{\bf G}^{T}\boldsymbol{\lambda}_{0}\geq{\bf n}$

which if ${\bf g}=0$ reduces to

${\bf N}{\bf M}^{{-1}}{\bf P}_{f}{\bf N}^{T}{\bf z}+{\bf N}{\bf M}^{{-1}}{\bf P}_{f}{\bf f}\geq{\bf n}.$

If the mixed LCP is expressed as

	$\displaystyle\left(\begin{matrix}{\bf M}&-{\bf G}^{T}&-{\bf N}^{T}\\ {\bf G}&0&0\\ {\bf N}&0&0\\ \end{matrix}\right)\left(\begin{matrix}{\bf x}\\ \boldsymbol{\lambda}\\ {\bf z}\end{matrix}\right)+\left(\begin{matrix}-{\bf f}\\ -{\bf g}\\ -{\bf n}\end{matrix}\right)=\left(\begin{matrix}0\\ 0\\ {\bf w}\end{matrix}\right),$
	$\displaystyle 0\leq{\bf z}\perp{\bf w}\geq 0,$

then we can partition this into

$\left(\begin{matrix}{\bf A}&-\bar{\bf N}^{T}\\ \bar{\bf N}&0\\ \end{matrix}\right)\left(\begin{matrix}{\bf y}\\ {\bf z}\end{matrix}\right)+\left(\begin{matrix}-{\bf b}\\ -{\bf n}\end{matrix}\right)=\left(\begin{matrix}0\\ {\bf w}\end{matrix}\right)$

with

${\bf A}\equiv\left(\begin{matrix}{\bf M}&-{\bf G}^{T}\\ {\bf G}&0\\ \end{matrix}\right),\quad{\bf y}\equiv\left(\begin{matrix}{\bf x}\\ \boldsymbol{\lambda}\end{matrix}\right),\quad\bar{\bf N}\equiv\left(\begin{matrix}{\bf N}&0\end{matrix}\right),\quad{\bf b}\equiv\left(\begin{matrix}{\bf f}\\ {\bf g}\end{matrix}\right)$

and create a regular LCP from

	$\displaystyle\bar{\bf N}{\bf A}^{{-1}}\bar{\bf N}^{T}{\bf z}+\bar{\bf N}{\bf A}^{{-1}}{\bf b}-{\bf n}={\bf w}$
	$\displaystyle 0\leq{\bf z}\perp{\bf w}\geq 0$

This will require $n+1$ solves of ${\bf A}$ , where $n$ is the number of rows of $\bar{\bf N}$ . Once we have solved for ${\bf z}$ , we can back solve for ${\bf x}$ and $\boldsymbol{\lambda}$ from

$\left(\begin{matrix}{\bf x}\\ \boldsymbol{\lambda}\end{matrix}\right)={\bf A}^{{-1}}\left({\bf b}+\bar{\bf N}^{T}{\bf z}\right)$

0.8 On demand pivoting for a mixed LCP formulation

The matrix $\bar{\bf N}{\bf A}^{{-1}}\bar{\bf N}^{T}$ described in the previous section can be created on demand, which may be advantageous if the number of pivots is smaller that the number of columns of $\bar{\bf N}$ .

For principal pivoting algorithms, the LCP matrix ${\bf M}$ and constant vector ${\bf q}$ are partitioned into

$\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}&{\bf M}_{{\alpha\beta}}\\ {\bf M}_{{\beta\alpha}}&{\bf M}_{{\beta\beta}}\end{matrix}\right)=\left(\begin{matrix}\bar{\bf N}_{{\alpha}}{\bf A}^{{-1}}\bar{\bf N}_{{\alpha}}^{T}&\bar{\bf N}_{{\alpha}}{\bf A}^{{-1}}\bar{\bf N}_{{\beta}}^{T}\\ \bar{\bf N}_{{\beta}}{\bf A}^{{-1}}\bar{\bf N}_{{\alpha}}^{T}&\bar{\bf N}_{{\beta}}{\bf A}^{{-1}}\bar{\bf N}_{{\beta}}^{T}\\ \end{matrix}\right),\quad\left(\begin{matrix}{\bf q}_{{\alpha}}\\ {\bf q}_{{\beta}}\end{matrix}\right)=\left(\begin{matrix}\bar{\bf N}_{{\alpha}}{\bf A}^{{-1}}{\bf b}\\ \bar{\bf N}_{{\beta}}{\bf A}^{{-1}}{\bf b}\\ \end{matrix}\right),$

with their pivoted versions given by

${\bf M}^{{\prime}}=\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}^{{-1}}&-{\bf M}_{{\alpha\alpha}}^{{-1}}{\bf M}_{{\alpha\beta}}\\ {\bf M}_{{\beta\alpha}}{\bf M}_{{\alpha\alpha}}^{{-1}}&{\bf M}_{{\beta\beta}}+{\bf M}_{{\beta\alpha}}{\bf M}_{{\alpha\beta}}^{{\prime}}\end{matrix}\right),\quad{\bf q}^{{\prime}}=\left(\begin{matrix}-{\bf M}_{{\alpha\alpha}}^{{-1}}{\bf q}_{{\alpha}}\\ {\bf q}_{{\beta}}+{\bf M}_{{\beta\alpha}}{\bf q}_{{\alpha}}^{{\prime}}\end{matrix}\right).$

(0.4)

The on-demand scheme works as follows. Column $j$ of ${\bf M}$ is computed (and stored) whenever $z_{j}$ is added to the basis for the first time. This means that we always have ${\bf M}_{{\alpha\alpha}}$ and ${\bf M}_{{\beta\alpha}}$ available, with a factorization of the former maintained so that we can easily compute products of ${\bf M}_{{\alpha\alpha}}^{{-1}}$ .

Pivoting algorithms require ${\bf q}^{{\prime}}$ and the $r$ -th column of ${\bf M}^{{\prime}}$ , which we denote by ${\bf m}_{r}^{{\prime}}$ . The former can be determined using ${\bf M}_{{\alpha\alpha}}$ and ${\bf M}_{{\beta\alpha}}$ , assuming that ${\bf q}$ has already been computed, as per (0.4). For ${\bf m}_{r}^{{\prime}}$ , if $r\in\alpha$ , then we have

${\bf m}_{r}^{{\prime}}=\left(\begin{matrix}{\bf m}_{{\alpha r}}^{{\prime}}\\ {\bf m}_{{\beta r}}^{{\prime}}\end{matrix}\right)=\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}^{{-1}}{\bf e}_{r}\\ {\bf M}_{{\beta\alpha}}{\bf m}_{{\alpha r}}^{{\prime}}\end{matrix}\right).$

Otherwise, if $r\in\beta$ , we compute ${\bf m}_{r}$ if necessary, and then

${\bf m}_{r}^{{\prime}}=\left(\begin{matrix}-{\bf M}_{{\alpha\alpha}}^{{-1}}{\bf m}_{{\alpha r}}\\ {\bf m}_{{\beta r}}+{\bf M}_{{\beta\alpha}}{\bf m}_{{\alpha r}}^{{\prime}}\end{matrix}\right).$

0.9 Solvers

Given a mechanical system defined by a mass matrix ${\bf M}$ , state ${\bf x}$ , and applied force ${\bf f}({\bf x},\dot{\bf x})$ , we can solve for its time evolution using several integrator types:

0.9.1 Symplectic Euler:

	$\displaystyle{\bf x}^{{k+1}}={\bf x}^{{k}}+h\dot{\bf x}^{{k+1}}$
	$\displaystyle{\bf M}\dot{\bf x}^{{k+1}}={\bf M}\dot{\bf x}^{{k}}+h{\bf f}^{{k}}$

0.9.2 Implicit Euler:

	$\displaystyle{\bf x}^{{k+1}}={\bf x}^{{k}}+h\dot{\bf x}^{{k+1}}$
	$\displaystyle\left({\bf M}-h\frac{\partial{\bf f}}{\partial\dot{\bf x}}-h^{2}\frac{\partial{\bf f}}{\partial\bf x}\right)\dot{\bf x}^{{k+1}}=\left({\bf M}-h\frac{\partial{\bf f}}{\partial\dot{\bf x}}\right)\dot{\bf x}^{{k}}+h{\bf f}^{{k}}$

The velocity expression can be expressed as

${\bf\hat{M}}\dot{\bf x}^{{k+1}}={\bf M}\dot{\bf x}^{{k}}+h\hat{\bf f}^{{k}}$

where

${\bf\hat{M}}\equiv\left({\bf M}-h\frac{\partial{\bf f}}{\partial\dot{\bf x}}-h^{2}\frac{\partial{\bf f}}{\partial\bf x}\right),\quad\hat{\bf f}^{{k}}\equiv{\bf f}^{{k}}-\frac{\partial{\bf f}}{\partial\dot{\bf x}}\dot{\bf x}^{k}$

or as

${\bf\hat{M}}\dot{\bf x}^{{k+1}}={\bf\hat{M}}\dot{\bf x}^{{k}}+h\tilde{\bf f}^{{k}}$

with

$\tilde{\bf f}^{{k}}\equiv{\bf f}^{{k}}+h\frac{\partial{\bf f}}{\partial\bf x}\dot{\bf x}^{k}.$

0.9.3 Trapezoidal:

For the trapezoidal rule, we determine $\dot{\bf x}^{{k+1}}$ from

$\dot{\bf x}^{{k+1}}=\dot{\bf x}^{k}+\frac{h}{2}{\bf M}^{{-1}}\left({\bf f}(\dot{\bf x}^{{k+1}},{\bf x}^{{k+1}})+{\bf f}(\dot{\bf x}^{{k}},{\bf x}^{{k}})\right)$

which leads to

	$\displaystyle{\bf x}^{{k+1}}={\bf x}^{{k}}+\frac{h}{2}(\dot{\bf x}^{{k+1}}+\dot{\bf x}^{{k}})$
	$\displaystyle\left({\bf M}-\frac{h}{2}\frac{\partial{\bf f}}{\partial\dot{\bf x}}-\frac{h^{2}}{4}\frac{\partial{\bf f}}{\partial\bf x}\right)\dot{\bf x}^{{k+1}}=\left({\bf M}-\frac{h}{2}\frac{\partial{\bf f}}{\partial\dot{\bf x}}+\frac{h^{2}}{4}\frac{\partial{\bf f}}{\partial\bf x}\right)\dot{\bf x}^{{k}}+h{\bf f}^{{k}}$

Again, the velocity expression can be expressed as

${\bf\hat{M}}\dot{\bf x}^{{k+1}}={\bf M}\dot{\bf x}^{{k}}+h\hat{\bf f}^{{k}}$

where

${\bf\hat{M}}\equiv\left({\bf M}-\frac{h}{2}\frac{\partial{\bf f}}{\partial\dot{\bf x}}-\frac{h^{2}}{4}\frac{\partial{\bf f}}{\partial\bf x}\right),\quad\hat{\bf f}^{{k}}\equiv{\bf f}^{{k}}-\frac{1}{2}\frac{\partial{\bf f}}{\partial\dot{\bf x}}\dot{\bf x}^{k}+\frac{h^{2}}{4}\frac{\partial{\bf f}}{\partial\bf x}\dot{\bf x}^{k},$

or as

${\bf\hat{M}}\dot{\bf x}^{{k+1}}={\bf\hat{M}}\dot{\bf x}^{{k}}+h\tilde{\bf f}^{{k}}$

with

$\tilde{\bf f}^{{k}}\equiv{\bf f}^{{k}}+\frac{h}{2}\frac{\partial{\bf f}}{\partial\bf x}\dot{\bf x}^{k}.$

0.9.4 Bridson-Marino:

This involves first computing an intermediate velocity $\dot{\bf x}^{{k+1/2}}$ :

$\left({\bf M}-\frac{h}{2}\frac{\partial{\bf f}}{\partial\dot{\bf x}}\right)\dot{\bf x}^{{k+1/2}}=\left({\bf M}-\frac{h}{2}\frac{\partial{\bf f}}{\partial\dot{\bf x}}\right)\dot{\bf x}^{{k}}+\frac{h}{2}{\bf f}^{{k}}.$

$\dot{\bf x}^{{k+1/2}}$ is now modified to satisfy constraints, and from this we compute ${\bf x}^{{k+1}}$ :

${\bf x}^{{k+1}}={\bf x}^{{k}}+h\dot{\bf x}^{{k+1/2}}.$

We can then determine ${\bf f}^{{\prime}}\equiv{\bf f}(\dot{\bf x}^{{k}},{\bf x}^{{k+1}})$ , and use this to solve for $\dot{\bf x}^{{k+1}}$ :

$\left({\bf M}-\frac{h}{2}\frac{\partial{\bf f}}{\partial\dot{\bf x}}\right)\dot{\bf x}^{{k+1}}=\left({\bf M}-\frac{h}{2}\frac{\partial{\bf f}}{\partial\dot{\bf x}}\right)\dot{\bf x}^{{k}}+\frac{h}{2}\left({\bf f}^{{k}}+{\bf f}^{{\prime}}\right).$

Note that the position Jacobian is not used.

0.10 Integration with Constraints

Adding bilateral constraints ${\bf G}$ and unilateral constraints ${\bf N}$ to the above systems, we obtain

	$\displaystyle\left(\begin{matrix}\hat{\bf M}&{\bf G}^{T}&{\bf N}^{T}\\ {\bf G}&0&0\\ {\bf N}&0&0\end{matrix}\right)\left(\begin{matrix}\dot{\bf x}^{{k+1}}\\ \boldsymbol{\lambda}\\ \boldsymbol{\theta}\end{matrix}\right)+\left(\begin{matrix}-{\bf M}\dot{\bf x}^{{k}}-h\hat{\bf f}^{{k}}\\ 0\\ 0\end{matrix}\right)=\left(\begin{matrix}0\\ 0\\ \boldsymbol{\nu}\end{matrix}\right)$
	$\displaystyle 0\leq\boldsymbol{\theta}\perp\boldsymbol{\nu}\geq 0$

If we add box friction to the system, we obtain

	$\displaystyle\left(\begin{matrix}\hat{\bf M}&{\bf G}^{T}&{\bf N}^{T}&{\bf D}^{T}\\ {\bf G}&0&0&0\\ {\bf N}&0&0&0\\ {\bf D}&0&0&0\\ \end{matrix}\right)\left(\begin{matrix}\dot{\bf x}^{{k+1}}\\ \boldsymbol{\lambda}\\ \boldsymbol{\theta}\\ \boldsymbol{\phi}\end{matrix}\right)+\left(\begin{matrix}-{\bf M}\dot{\bf x}^{{k}}-h\hat{\bf f}^{{k}}\\ 0\\ 0\\ \par \end{matrix}\right)=\left(\begin{matrix}0\\ 0\\ \boldsymbol{\nu}\\ \boldsymbol{\sigma}\end{matrix}\right)$		(0.5)
	$\displaystyle 0\leq\boldsymbol{\theta}\perp\boldsymbol{\nu}\geq 0,\quad 0\leq\boldsymbol{\phi}-\underline{\boldsymbol{\phi}}\perp\boldsymbol{\sigma}\geq 0,\quad 0\leq\overline{\boldsymbol{\phi}}-\boldsymbol{\phi}\perp\boldsymbol{\sigma}\leq 0$		(0.6)

where $\overline{\boldsymbol{\phi}}$ and $\underline{\boldsymbol{\phi}}$ are box bounds on $\boldsymbol{\phi}$ determined from the friction coefficients $\mu$ and an estimate of $\boldsymbol{\theta}$ .

If we wish Anitescu/Stewart friction, then we obtain a more elaborate system

	$\displaystyle\left(\begin{matrix}\hat{\bf M}&-{\bf G}^{T}&-{\bf N}^{T}&-{\bf D}^{T}&0\\ {\bf G}&0&0&0&0\\ {\bf N}&0&0&0&0\\ {\bf D}&0&0&0&{\bf E}\\ 0&0&\boldsymbol{\mu}&-{\bf E}^{T}&0\\ \end{matrix}\right)\left(\begin{matrix}\dot{\bf x}^{{k+1}}\\ \boldsymbol{\lambda}\\ \boldsymbol{\theta}\\ \boldsymbol{\phi}\\ \boldsymbol{\xi}\end{matrix}\right)+\left(\begin{matrix}-{\bf M}\dot{\bf x}^{{k}}-h\hat{\bf f}^{{k}}\\ 0\\ 0\\ 0\\ \par \end{matrix}\right)=\left(\begin{matrix}0\\ 0\\ \boldsymbol{\nu}\\ \boldsymbol{\sigma}\\ \boldsymbol{\gamma}\end{matrix}\right)$
	$\displaystyle 0\leq\boldsymbol{\theta}\perp\boldsymbol{\nu}\geq 0,\quad 0\leq\boldsymbol{\phi}\perp\boldsymbol{\sigma}\geq 0,\quad 0\leq\boldsymbol{\xi}\perp\boldsymbol{\gamma}\geq 0$

The matrix ${\bf E}$ is defined such that ${\bf E}_{{ij}}=1$ for all $i$ corresponding to the frictional wrenches ${\bf d}_{i}$ of the same contact. It enforces the friction cone and maximal dissipation constraints: for a given contact with friction wrenches ${\bf d}_{k}$ , we have

$\mu\theta-\sum\phi _{k}=\gamma=\left\{\begin{array}[]{ll}=0&\mbox{on the friction cone}\\ >0&\mbox{inside the friction cone}\end{array}\right.$

${\bf d}_{k}{\bf v}+\xi=\sigma=\left\{\begin{array}[]{ll}=0&\mbox{if ${\bf d}_{k}{\bf v}$ is maximally negative}\\ >0&\mbox{if ${\bf d}_{k}{\bf v}$ is {\it not} maximally negative}\end{array}\right.$

Then from the friction cone constraint we have

$\xi>0\Rightarrow\mbox{tangential motion}\Rightarrow\mbox{on the cone}\Rightarrow\gamma=0$

and from maximal dissiption we get

$\gamma>0\Rightarrow\mbox{inside the friction cone}\Rightarrow\mbox{no tangential velocity}\Rightarrow\xi=0$

0.11 Softening Constraints with Regularization

This material uses ideas from [].

Consider a general KKT system such as that described by (0.3), and let $\boldsymbol{\phi}({\bf q})=0$ denote the constraint function for which ${\bf G}\equiv\partial\boldsymbol{\phi}/\partial{\bf q}$ is the derivative.

Instead of enforcing constraints exactly, we can enforce them using a potential function based on the distance from the constraint (defined by $\boldsymbol{\phi}({\bf q})$ ), such as

$U({\bf q})=\frac{1}{2}\boldsymbol{\phi}({\bf q})^{T}\boldsymbol{\alpha}^{{-1}}\boldsymbol{\phi}({\bf q}),$

where $\boldsymbol{\alpha}$ is an inverse stiffness matrix. The constraint forces ${\bf f}_{c}$ arising from this are given by

${\bf f}_{c}=-\frac{\partial U}{\partial{\bf q}}=-{\bf G}^{T}\boldsymbol{\alpha}^{{-1}}\boldsymbol{\phi}({\bf q})={\bf G}^{T}\boldsymbol{\lambda}$

where $\boldsymbol{\lambda}\equiv-\boldsymbol{\alpha}^{{-1}}\boldsymbol{\phi}({\bf q})$ now gives the “soft” constraint force. We can also add a damping term $\boldsymbol{\beta}$ , so that

$\boldsymbol{\lambda}=-\boldsymbol{\alpha}^{{-1}}(\boldsymbol{\phi}({\bf q})+\boldsymbol{\beta}\dot{\boldsymbol{\phi}}).$

(0.7)

(Note that here the full damping term is $\boldsymbol{\alpha}^{{-1}}\boldsymbol{\beta}$ ).

Now we will integrate (0.7) into a KKT system where we are solving for updated velocity values ${\bf v}_{{k+1}}$ . Following the derivation in [], we consider the average value $\bar{\boldsymbol{\lambda}}$ of $\boldsymbol{\lambda}$ over the time step, or more specifically,

$\boldsymbol{\alpha}\bar{\boldsymbol{\lambda}}=-\bar{\boldsymbol{\phi}}-\boldsymbol{\beta}\bar{\dot{\boldsymbol{\phi}}}.$

Using the approximations $\bar{\boldsymbol{\phi}}\approx(\boldsymbol{\phi}_{{k+1}}+\boldsymbol{\phi}_{k})/2$ , $\bar{\dot{\boldsymbol{\phi}}}\approx{\bf G}{\bf v}_{{k+1}}$ , and finally $\boldsymbol{\phi}_{{k+1}}\approx\boldsymbol{\phi}_{k}+h{\bf G}{\bf v}_{{k+1}}$ , where $h$ is the step size, we obtain

$\boldsymbol{\alpha}\bar{\boldsymbol{\lambda}}=-\boldsymbol{\phi}_{k}+\left(\frac{h}{2}+\boldsymbol{\beta}\right)\bar{\dot{\boldsymbol{\phi}}}\approx-\boldsymbol{\phi}_{k}+\left(\frac{h}{2}+\boldsymbol{\beta}\right){\bf G}{\bf v}_{{k+1}}.$

(0.8)

Now defining $\boldsymbol{\gamma}\equiv(1/2+\boldsymbol{\beta}/h)^{{-1}}$ , we can rearrange this as

${\bf G}{\bf v}_{{k+1}}+\frac{\boldsymbol{\gamma}\boldsymbol{\alpha}}{h^{2}}\tilde{\boldsymbol{\lambda}}=-\frac{\boldsymbol{\gamma}\boldsymbol{\phi}_{k}}{h},$

where $\tilde{\boldsymbol{\lambda}}\equiv\bar{\boldsymbol{\lambda}}$ denotes the constraint impulse we which to solve for along with ${\bf v}_{{k+1}}$ . This fits into a KKT framework as

$\left(\begin{matrix}{\bf M}&-{\bf G}^{T}\\ {\bf G}&\boldsymbol{\gamma}\boldsymbol{\alpha}/h^{2}\end{matrix}\right)\left(\begin{matrix}{\bf v}_{{k+1}}\\ \tilde{\boldsymbol{\lambda}}\end{matrix}\right)\;=\;\left(\begin{matrix}{\bf b}\\ -\boldsymbol{\gamma}\boldsymbol{\phi}_{k}/h\end{matrix}\right).$

(0.9)

Note in particular that if $\boldsymbol{\beta}=0$ , then $\boldsymbol{\gamma}=2$ , and so

$\bar{\boldsymbol{\lambda}}=-\frac{\boldsymbol{\alpha}^{{-1}}}{2}(h{\bf G}{\bf v}_{{k+1}}+2\boldsymbol{\phi}_{k})\approx-\frac{\boldsymbol{\alpha}^{{-1}}}{2}(\boldsymbol{\phi}_{{k+1}}+\boldsymbol{\phi}_{k}),$

implying that $\bar{\boldsymbol{\lambda}}$ is approximatelty equal to the average value of $\boldsymbol{\phi}$ during the step, times the restoring stiffness.

It is also possible to add regularization to unilateral constraints,

$\left(\begin{matrix}{\bf M}&-{\bf N}^{T}\\ {\bf N}&\boldsymbol{\gamma}\boldsymbol{\alpha}/h^{2}\end{matrix}\right)\left(\begin{matrix}{\bf v}_{{k+1}}\\ \tilde{\bf z}\end{matrix}\right)+\left(\begin{matrix}-{\bf b}\\ \boldsymbol{\gamma}\boldsymbol{\phi}_{k}/h\end{matrix}\right)=\left(\begin{matrix}0\\ {\bf w}\end{matrix}\right),\quad 0\leq\tilde{\bf z}\perp{\bf w}\geq 0.$

(0.10)

with $\tilde{\bf z}\equiv h\bar{\bf z}$ denoting the unilateral constraint impulse and $\bar{\bf z}$ denoting the average constraint force. To understand how this will behave, we work backwards toward the form of (0.8) to obtain

$\left(\frac{h}{2}+\boldsymbol{\beta}\right){\bf w}=\boldsymbol{\alpha}\bar{\bf z}+\boldsymbol{\phi}_{k}+\left(\frac{h}{2}+\boldsymbol{\beta}\right)\bar{\dot{\boldsymbol{\phi}}}.$

Now, by the complementarity conditions, if $\bar{\bf z}>0$ , then ${\bf w}=0$ and so

$\boldsymbol{\alpha}\bar{\bf z}=-\boldsymbol{\phi}_{k}-\left(\frac{h}{2}+\boldsymbol{\beta}\right)\bar{\dot{\boldsymbol{\phi}}}>0.$

Otherwise, if ${\bf w}>0$ , then $\bar{\bf z}=0$ and so

$\boldsymbol{\phi}_{k}+\left(\frac{h}{2}+\boldsymbol{\beta}\right)\bar{\dot{\boldsymbol{\phi}}}>0.$

Note that for unilateral constraints, the damping term might introduce some anomolous results. If $\phi _{k}$ and $\dot{\phi}_{k}$ are both negative, then ${\bf z}>0$ , while if they are both positive, then ${\bf z}=0$ , as we would expect. However, if $\phi _{k}<0$ and $\dot{\phi}_{k}>0$ , then a large value of $\boldsymbol{\beta}$ can cause ${\bf z}=0$ even if the interpenetration continues (i.e., $\phi _{{k+1}}<0$ ) at the end of the time step.

0.12 Jacobians for Axial Springs

An axial spring between two points ${\bf x}_{1}$ and ${\bf x}_{2}$ exerts forces ${\bf f}_{1}$ and ${\bf f}_{2}$ onto the points given by

${\bf f}\equiv\left(\begin{matrix}{\bf f}_{1}\\ {\bf f}_{2}\end{matrix}\right)=\left(\begin{matrix}F(l,\dot{l}){\bf u}\\ -F(l,\dot{l}){\bf u}\end{matrix}\right)$

where $F$ is a scalar describing the spring force and

${\bf u}\equiv\frac{{\bf x}_{2}-{\bf x}_{1}}{l},\quad l\equiv\|{\bf x}_{2}-{\bf x}_{1}\|.$

Differentiating this relationship with respect to node positions and velocities gives the position and velocity Jacobians:

$\frac{\partial{\bf f}}{\partial{\bf x}}\equiv\left(\begin{matrix}{\partial{\bf f}_{1}}/{\partial{\bf x}_{1}}&{\partial{\bf f}_{1}}/{\partial{\bf x}_{2}}\\ {\partial{\bf f}_{2}}/{\partial{\bf x}_{1}}&{\partial{\bf f}_{2}}/{\partial{\bf x}_{2}}\end{matrix}\right)$

$\frac{\partial{\bf f}}{\partial\dot{\bf x}}\equiv\left(\begin{matrix}{\partial{\bf f}_{1}}/{\partial\dot{\bf x}_{1}}&{\partial{\bf f}_{1}}/{\partial\dot{\bf x}_{2}}\\ {\partial{\bf f}_{2}}/{\partial\dot{\bf x}_{1}}&{\partial{\bf f}_{2}}/{\partial\dot{\bf x}_{2}}\end{matrix}\right)$

Because of symmetry, it is easy to show that

$\frac{\partial{\bf f}_{2}}{\partial{\bf x}_{2}}=\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}\quad\text{and}\quad\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{2}}=\frac{\partial{\bf f}_{2}}{\partial{\bf x}_{1}}=-\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}},$

and similarly for the velocity Jacobian, and so it is sufficient to analyze ${\partial{\bf f}_{1}}/{\partial\dot{\bf x}_{1}}$ and ${\partial{\bf f}_{1}}/{\partial\dot{\bf x}_{1}}$ . Applying the product rule to these gives

$\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}={\bf u}\frac{\partial F}{\partial{\bf x}_{1}}+F\frac{\partial{\bf u}}{\partial{\bf x}_{1}},\quad\frac{\partial{\bf f}_{1}}{\partial\dot{\bf x}_{1}}={\bf u}\frac{\partial F}{\partial\dot{\bf x}_{1}}.$

Applying the chain rule to $F(l,\dot{l})$ gives

$\frac{\partial F}{\partial{\bf x}_{1}}=\frac{\partial F}{\partial l}\frac{\partial l}{\partial{\bf x}_{1}}+\frac{\partial F}{\partial\dot{l}}\frac{\partial\dot{l}}{\partial{\bf x}_{1}},\quad\frac{\partial F}{\partial\dot{\bf x}_{1}}=\frac{\partial F}{\partial\dot{l}}\frac{\partial\dot{l}}{\partial\dot{\bf x}_{1}}$

To continue our analysis, we utilize the following:

	$\displaystyle\dot{l}$	$\displaystyle={\bf u}^{T}(\dot{\bf x}_{2}-\dot{\bf x}_{1})=(\dot{\bf x}_{2}-\dot{\bf x}_{1})^{T}{\bf u},$
	$\displaystyle\frac{\partial l}{\partial{\bf x}_{1}}$	$\displaystyle=-{\bf u}^{T},$
	$\displaystyle\frac{\partial{\bf u}}{\partial{\bf x}_{1}}$	$\displaystyle=-\frac{1}{l}{\bf I}+({\bf x}_{2}-{\bf x}_{1})\frac{\partial}{\partial{\bf x}_{1}}(\frac{1}{l})=-\frac{1}{l}({\bf I}-{\bf u}{\bf u}^{T}),$
	$\displaystyle\frac{\partial\dot{l}}{\partial{\bf x}_{1}}$	$\displaystyle=(\dot{\bf x}_{2}-\dot{\bf x}_{1})^{T}\frac{\partial{\bf u}}{\partial{\bf x}_{1}}=-\frac{1}{l}(\dot{\bf x}_{2}-\dot{\bf x}_{1})^{T}({\bf I}-{\bf u}{\bf u}^{T}),$
	$\displaystyle\frac{\partial\dot{l}}{\partial\dot{\bf x}_{1}}$	$\displaystyle=-{\bf u}^{T}$

Then if we define ${\bf U}_{n}\equiv{\bf u}{\bf u}^{T}$ and ${\bf U}_{p}\equiv({\bf I}-{\bf u}{\bf u}^{T})$ (which are the matrices that project a vector onto the direction of ${\bf u}$ and the plane perpendicular to ${\bf u}$ , respectively), and assume that $\partial F/\partial l$ and $\partial F/\partial\dot{l}$ are given, we obtain

$\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}=-\frac{\partial F}{\partial l}{\bf U}_{n}-\frac{1}{l}\frac{\partial F}{\partial\dot{l}}{\bf u}(\dot{\bf x}_{2}-\dot{\bf x}_{1})^{T}{\bf U}_{p}-\frac{F}{l}{\bf U}_{p}$

$\frac{\partial{\bf f}_{1}}{\partial\dot{x}_{1}}=-\frac{\partial F}{\partial\dot{l}}{\bf U}_{n}$

0.12.1 Linear Axial Springs

For a linear axial spring, we have

$F=k(l-r)+d\dot{l}$

where $k$ and $d$ are the stiffness and damping coefficients, and $r$ is the rest lenth. Then

$\frac{\partial F}{\partial l}=k,\quad\frac{\partial F}{\partial\dot{l}}=d,$

and

	$\displaystyle\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}=-k{\bf U}_{n}-\frac{d}{l}{\bf u}(\dot{\bf x}_{2}-\dot{\bf x}_{1})^{T}{\bf U}_{p}-\frac{k(l-r)+d\dot{l}}{l}{\bf U}_{p},$
	$\displaystyle\frac{\partial{\bf f}_{1}}{\partial\dot{\bf x}_{1}}=-d{\bf U}_{n}$

If $d=0$ , this reduces to

$\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}=-k{\bf U}_{n}-\frac{k(l-r)}{l}{\bf U}_{p}\quad\text{and}\quad\frac{\partial{\bf f}_{1}}{\partial\dot{\bf x}_{1}}=0$

0.13 Spring Jacobians Formulated using Stress

If we reformulate the results of Section 0.12 using force-per-length, which we will call a “stress” $\sigma$ defined by

$\sigma(l,\dot{l})=\frac{F(l,\dot{l})}{l},$

then we can replace the expression ${\bf f}_{1}=F(l,\dot{l}){\bf u}$ with

${\bf f}_{1}=\sigma(l,\dot{l})\Delta{\bf x}$

where $\Delta x\equiv{\bf x}_{2}-{\bf x}_{1}$ . This approach is closer to the finite element formulation for a two-element truss node and can simplify the Jacobian calculations.

In particular, we now have

$\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}=\Delta{\bf x}\frac{\partial\sigma}{\partial{\bf x}_{1}}+\sigma\frac{\partial\Delta{\bf x}}{\partial{\bf x}_{1}}\\ =\Delta{\bf x}\frac{\partial\sigma}{\partial{\bf x}_{1}}-\sigma{\bf I}$

Similar to before, we have

$\frac{\partial\sigma(l,\dot{l})}{\partial{\bf x}_{1}}=\frac{\partial\sigma}{\partial l}\frac{\partial l}{\partial{\bf x}_{1}}+\frac{\partial\sigma}{\partial\dot{l}}\frac{\partial\dot{l}}{\partial{\bf x}_{1}}$

If there is no dependence on $\dot{l}$ , and $\sigma(l)$ is linear so that

$\sigma(l)=\frac{k(l-r)}{l}$

then

$\frac{\partial\sigma}{\partial l}=\frac{\partial}{\partial l}\left(\frac{k(l-r)}{l}\right)=\frac{\partial}{\partial l}\left(\frac{-kr}{l}\right)=-\frac{kr}{l^{2}}{\bf u}^{T}$

and so $\partial{\bf f}_{1}/\partial{\bf x}_{1}$ reduces to

$\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}=-\frac{kr}{l}{\bf u}{\bf u}^{T}-\sigma{\bf I}.$

If the rest length $r$ is zero, then we have the very simple result

$\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}=-k{\bf I}$

which is consistent with Section 0.12.1.

Most FEM formulations use a non-zero rest length, consistent with having a finite size reference element. If we define a quadratic strain $\varepsilon$ by

$\varepsilon=\frac{1}{2}\left(\frac{l^{2}-r^{2}}{r^{2}}\right)$

and relate this linearly to $\sigma$ using

$\sigma=k\varepsilon$

then we can show that

$\frac{\partial\varepsilon}{\partial{\bf x}_{1}}=\frac{l}{r^{2}}\frac{\partial l}{\partial{\bf x}_{1}}=-\frac{1}{r^{2}}\Delta{\bf x}^{T}$

and so

$\frac{\partial{\bf f}_{1}}{\partial{\bf x}_{1}}=-\frac{k}{r^{2}}\Delta{\bf x}\Delta{\bf x}^{T}-\sigma{\bf I}$

In FEM parlance, the first term is the material stiffness, while the second term is the geometric stiffness.

0.14 Jacobians for Multi-Point Springs

A multi-point spring is an axial spring with extra via points that act like generalized pulleys and alter the spring’s direction (Figure 0.1). The tension $F$ within the spring is a function of the entire length $L$ and length derivative $\dot{L}$ .

Figure 0.1: A multi-point spring with two end points and two via points.

For the following discussion, we will assume that a multi-spring is composed of $n$ segments, defined by $n+1$ points ${\bf x}_{0},\ldots,{\bf x}_{n}$ , where ${\bf x}_{0}$ and ${\bf x}_{n}$ are the end points and the remaining ${\bf x}_{i}$ are the via points. The segments are associated with direction vectors ${\bf u}_{0},\ldots,{\bf u}_{{n-1}}$ and lengths $l_{0},\ldots,l_{{n-1}}$ , defined by

${\bf u}_{i}\equiv\frac{{\bf x}_{{i+1}}-{\bf x}_{i}}{l_{i}},\quad l_{i}\equiv\|{\bf x}_{{i+1}}-{\bf x}_{i}\|.$

(0.11)

The forces on the end points and via points are given by

${\bf f}_{0}=F{\bf u}_{0},\quad{\bf f}_{i}=F({\bf u}_{i}-{\bf u}_{{i-1}}),\quad{\bf f}_{n}=-F{\bf u}_{{n-1}},$

and the total length $L$ and its derivative is given by

$L=\sum _{{i=0}}^{{n-1}}l_{i}\quad\text{and}\quad\dot{L}=\sum _{{i=0}}^{{n-1}}(\dot{\bf x}_{{i+1}}-\dot{\bf x}_{i})^{T}{\bf u}_{i}.$

(0.12)

Following the discussion of Section 0.12, we then have, for a via point,

$\frac{\partial{\bf f}_{i}}{\partial{\bf x}_{j}}=({\bf u}_{i}-{\bf u}_{{i-1}})\frac{\partial F}{\partial{\bf x}_{j}}+F\left(\frac{\partial{\bf u}_{i}}{\partial{\bf x}_{j}}-\frac{\partial{\bf u}_{{i-1}}}{\partial{\bf x}_{j}}\right).$

From (0.11), it can be determined that for $j=i$ and $j=i+1$ we have

$\frac{\partial{\bf u}_{i}}{\partial{\bf x}_{i}}=-\frac{1}{l_{i}}{\bf P}_{i},\quad\frac{\partial{\bf u}_{i}}{\partial{\bf x}_{{i+1}}}=\frac{1}{l_{i}}{\bf P}_{i},\quad\text{and}\quad\frac{\partial{\bf u}_{i}}{\partial{\bf x}_{j}}=0\quad(j\neq i,i+1)$

(0.13)

where ${\bf P}_{i}\equiv{\bf I}-{\bf u}_{i}{\bf u}_{i}^{T}$ . For $\partial{\bf f}_{i}/\partial\dot{\bf x}_{j}$ , we have simply

$\frac{\partial{\bf f}_{i}}{\partial\dot{\bf x}_{j}}=({\bf u}_{i}-{\bf u}_{{i-1}})\frac{\partial F}{\partial\dot{\bf x}_{{j}}}.$

Because $F$ is a function of $L$ and $\dot{L}$ , it is affected by the position and velocity of each point. In particular, from the discussion of Section 0.12, we have

$\frac{\partial F}{\partial{\bf x}_{i}}=\frac{\partial F}{\partial L}\frac{\partial L}{\partial{\bf x}_{i}}+\frac{\partial F}{\partial\dot{L}}\frac{\partial\dot{L}}{\partial{\bf x}_{i}}\quad\text{and}\quad\frac{\partial F}{\partial\dot{\bf x}_{i}}=\frac{\partial F}{\partial\dot{L}}\frac{\partial\dot{L}}{\partial\dot{\bf x}_{i}}.$

From (0.12), we can determine $\partial L/\partial{\bf x}_{i}$ , $\partial\dot{L}/\partial{\bf x}_{i}$ , and $\partial\dot{L}/\partial\dot{\bf x}_{i}$ :

	$\displaystyle\frac{\partial L}{\partial{\bf x}_{i}}$	$\displaystyle=({\bf u}_{{i-1}}^{T}-{\bf u}_{i}^{T}),$
	$\displaystyle\frac{\partial\dot{L}}{\partial{\bf x}_{i}}$	$\displaystyle=(\dot{\bf x}_{i}-\dot{\bf x}_{{i-1}})^{T}\frac{\partial{\bf u}_{{i-1}}}{\partial{\bf x}_{i}}+(\dot{\bf x}_{{i+1}}-\dot{\bf x}_{{i}})^{T}\frac{\partial{\bf u}_{{i}}}{\partial{\bf x}_{i}}$
		$\displaystyle=\frac{1}{l_{{i-1}}}(\dot{\bf x}_{i}-\dot{\bf x}_{{i-1}})^{T}{\bf P}_{{i-1}}-\frac{1}{l_{i}}(\dot{\bf x}_{{i+1}}-\dot{\bf x}_{{i}})^{T}{\bf P}_{i},$
	$\displaystyle\frac{\partial\dot{L}}{\partial\dot{\bf x}_{i}}$	$\displaystyle=({\bf u}_{{i-1}}^{T}-{\bf u}_{i}^{T}).$

More generally, for multi-point springs, we can define the notion of active segments, which contribute to $L$ and $\dot{L}$ , and passive segments, which do not. Let the set of active segments be denoted by ${\cal A}$ , and for notational convenience, consider the endpoints ${\bf x}_{0}$ and ${\bf x}_{n}$ to be connected to fictitous passive segments $-1$ and $n+1$ , respectively. The $L$ and $\dot{L}$ are then redefined by

$L=\sum _{{i\in{\cal A}}}l_{i}\quad\text{and}\quad\dot{L}=\sum _{{i\in{\cal A}}}(\dot{\bf x}_{{i+1}}-\dot{\bf x}_{i})^{T}{\bf u}_{i}.$

(0.14)

Now if we define ${\bf v}_{i}$ such that

${\bf v}_{0}\equiv{\bf u}_{0},\quad{\bf v}_{n}\equiv-{\bf u}_{{n-1}},\quad{\bf v}_{i}\equiv{\bf u}_{i}-{\bf u}_{{i-1}}\quad(i\neq 0,n),$

then for all $i$ ,

${\bf f}_{i}=F{\bf v}_{i}$

and it is also straightforward to show that

$\frac{\partial L}{\partial{\bf x}_{i}}=\frac{\partial\dot{L}}{\partial\dot{\bf x}_{i}}=\begin{cases}{\bf u}_{{i-1}}^{T},&{i-1}\in{\cal A},i\notin{\cal A}\\ -{\bf u}_{{i}}^{T},&{i-1}\notin{\cal A},i\in{\cal A}\\ -{\bf v}_{{i}}^{T},&{i-1}\in{\cal A},i\in{\cal A}\\ 0,&\text{otherwise}.\end{cases}$

Moreover, if for $i\in{\cal A}$ we define

${\bf y}_{i}\equiv\frac{1}{l_{i}}\frac{\partial F}{\partial\dot{L}}(\dot{\bf x}_{{i+1}}-\dot{\bf x}_{i})^{T}{\bf P}_{i}$

and ${\bf y}_{i}\equiv 0$ otherwise, then

$\frac{\partial F}{\partial{\bf x}_{i}}=\frac{\partial F}{\partial L}\frac{\partial L}{\partial{\bf x}_{i}}+{\bf y}_{{i-1}}-{\bf y}_{i}.$

Then then leads to

$\frac{\partial{\bf f}_{i}}{\partial\dot{\bf x}_{j}}=\frac{\partial F}{\partial\dot{L}}{\bf v}_{i}\frac{\partial\dot{L}}{\partial\dot{\bf x}_{i}}$

and

$\frac{\partial{\bf f}_{i}}{\partial{\bf x}_{j}}={\bf v}_{i}\frac{\partial F}{\partial{\bf x}_{j}}+F\frac{\partial{\bf u}_{i}}{\partial{\bf x}_{j}}-F\frac{\partial{\bf u}_{{i-1}}}{\partial{\bf x}_{j}}.$

where $\partial{\bf u}_{i}/\partial{\bf x}_{j}$ is given by (0.13).

0.15 Gauss Seidel and KKT Systems

Consider a KKT system defined by

	$\displaystyle{\bf v}={\bf v}_{0}+{\bf M}^{{-1}}{\bf N}^{T}\boldsymbol{\lambda},$
	$\displaystyle{\bf N}{\bf v}={\bf c}$

Solving this requires solving for $\boldsymbol{\lambda}$ according to

$({\bf N}{\bf M}^{{-1}}{\bf N}^{T})\boldsymbol{\lambda}={\bf c}-{\bf N}{\bf v}_{0}.$

(0.15)

Now, as described in Wikipedia, Gauss Seidel involves solving ${\bf A}{\bf x}={\bf b}$ by decomposing ${\bf A}$ into ${\bf D}+{\bf L}+{\bf U}$ , where ${\bf L}$ and ${\bf U}$ are strictly lower and upper triangular, and then applying the iteration

${\bf x}^{{(k+1)}}=({\bf D}+{\bf L})^{{-1}}({\bf b}-{\bf U}{\bf x}^{{(k)}}).$

(0.16)

Component-wise, this takes the form

$x_{i}^{{(k+1)}}=\frac{1}{a_{{ii}}}\left(b_{i}-\sum _{{j<i}}a_{{ij}}x_{j}^{{(k+1)}}-\sum _{{j>i}}a_{{ij}}x_{j}^{{(k)}}\right),i=1,\ldots n.$

(0.17)

Applying this to (0.15) gives

	$\displaystyle\lambda _{i}^{{(k+1)}}$	$\displaystyle={\left({\bf N}_{i}{\bf M}^{{-1}}{\bf N}_{i}^{T}\right)}^{{-1}}\left(c_{i}-{\bf N}_{i}{\bf v}_{0}-\sum _{{j<i}}{\bf N}_{i}{\bf M}^{{-1}}{\bf N}_{j}^{T}\lambda _{j}^{{(k+1)}}-\sum _{{j>i}}{\bf N}_{i}{\bf M}^{{-1}}{\bf N}_{j}^{T}\lambda _{j}^{{(k)}}\right)$
		$\displaystyle={\left({\bf N}_{i}{\bf M}^{{-1}}{\bf N}_{i}^{T}\right)}^{{-1}}\left(c_{i}-{\bf N}_{i}\left({\bf v}_{0}-\sum _{{j<i}}{\bf M}^{{-1}}{\bf N}_{j}^{T}\lambda _{j}^{{(k+1)}}-\sum _{{j>i}}{\bf M}^{{-1}}{\bf N}_{j}^{T}\lambda _{j}^{{(k)}}\right)\right)$
		$\displaystyle={\left({\bf N}_{i}{\bf M}^{{-1}}{\bf N}_{i}^{T}\right)}^{{-1}}\left(c_{i}-{\bf N}_{i}{\bf v}^{{(i)}}\right),$

where ${\bf v}^{{(i)}}$ is the total velocity updated for the $i$ -th iteration step. The term $({\bf N}_{i}{\bf M}^{{-1}}{\bf N}_{i}^{T})^{{-1}}$ represents the solution of constraint $i$ in isolation; this term will be a scalar if the constraints are all one-dimensional.

0.16 A Gauss Seidel Friction Iteration

We describe here a Gauss-Seidel step to handle friction at a single contact point. Assume that the constraint directions for this point are given by ${\bf D}$ (which in general, will be a $2\times n$ sparse matrix, where each row is generated by the two tangential degrees of freedom at the contact point, mapped onto the dynamic components associated with the contact).

The tangential velocity components ${\bf v}_{t}$ are then given by

${\bf v}_{t}={\bf D}{\bf v}.$

In response, a friction impulse $\boldsymbol{\phi}$ is applied to ${\bf D}^{T}$ , resulting in a change in tangential velocity given by

$\Delta{\bf v}_{t}=w\,\boldsymbol{\phi},\quad w\equiv{\bf D}{\bf M}^{{-1}}{\bf D}^{T}$

Assuming that the size of the normal impulse $\lambda$ is known, then the maximum possible magnitude of the friction impulse is $\boldsymbol{\phi}_{\text{max}}\equiv\mu\lambda$ , where $\mu$ is the friction coefficient. The friction impulse $\boldsymbol{\phi}$ is applied to reduce ${\bf v}_{t}$ as mush as possible given this limit:

$\boldsymbol{\phi}=\begin{cases}-\boldsymbol{\phi}_{\text{max}}&v_{t}\geq w\,\boldsymbol{\phi}_{\text{max}}\\ \boldsymbol{\phi}_{\text{max}}&v_{t}\leq w\,\boldsymbol{\phi}_{\text{max}}\\ -v_{t}/w&\text{otherwise}\end{cases}$

This is equivalent to the box friction conditions described in (0.6).

0.17 Null-space reduction of the KKT system for attachments

A component $k$ is attached to one or more active master components if its velocity can be directly determined from its masters:

${\bf v}_{{k}}+{\bf G}_{{k\alpha}}{\bf v}_{{\alpha}}=0$

(0.18)

where ${\bf v}_{{k}}$ is the attached component’s velocity, and ${\bf v}_{{\alpha}}$ is the velocity of all the active components. ${\bf G}_{{k\alpha}}$ will be sparse except for entries corresponding to the master components to which $k$ is attached.

Let the set of active components be denoted by $\alpha$ , and the set of attached points be denoted by $\beta$ . Letting ${\bf G}_{{\beta\alpha}}$ denote the matrix formed from ${\bf G}_{{k\alpha}}$ for all components, we have

${\bf I}{\bf v}_{{\beta}}+{\bf G}_{{\beta\alpha}}{\bf v}_{{\alpha}}=0$

as the constraint that enforces all attachments. Adding this to the KKT system, and partitioning that system into attached components and active components, yields

$\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}&{\bf M}_{{\alpha\beta}}&{\bf G}_{{\alpha\alpha}}^{T}&{\bf G}_{{\beta\alpha}}^{T}\\ {\bf M}_{{\beta\alpha}}&{\bf M}_{{\beta\beta}}&{\bf G}_{{\alpha\beta}}^{T}&{\bf I}\\ {\bf G}_{{\alpha\alpha}}&{\bf G}_{{\alpha\beta}}&{\bf R}_{{\alpha}}&0\\ {\bf G}_{{\beta\alpha}}&{\bf I}&0&0\\ \end{matrix}\right)\left(\begin{matrix}{\bf v}_{{\alpha}}\\ {\bf v}_{{\beta}}\\ \boldsymbol{\lambda}_{{\alpha}}\\ \boldsymbol{\lambda}_{{\beta}}\end{matrix}\right)=\left(\begin{matrix}{\bf f}_{{\alpha}}\\ {\bf f}_{{\beta}}\\ {\bf g}_{{\alpha}}\\ {\bf g}_{{\beta}}\end{matrix}\right)$

(0.19)

where ${\bf R}_{{\alpha}}$ is a diagonal regularization matrix for the constraints (since attachments are hard constraints, no regularization is included).

The identity submatrices make it easy to solve for ${\bf v}_{{\beta}}$ and $\boldsymbol{\lambda}_{{\beta}}$ :

	$\displaystyle{\bf v}_{{\beta}}$	$\displaystyle={\bf g}_{{\beta}}-{\bf G}_{{\beta\alpha}}{\bf v}_{{\alpha}}$
	$\displaystyle\boldsymbol{\lambda}_{{\beta}}$	$\displaystyle={\bf f}_{{\beta}}-{\bf M}_{{\beta\alpha}}{\bf v}_{{\alpha}}+{\bf M}_{{\beta\beta}}{\bf G}_{{\beta\alpha}}{\bf v}_{{\alpha}}-{\bf M}_{{\beta\beta}}{\bf g}_{{\beta}}-{\bf G}_{{\alpha\beta}}^{T}\boldsymbol{\lambda}_{{\alpha}}$

and hence reduce the system to

$\left(\begin{matrix}{\bf M}^{{\prime}}&&{\bf G}^{{\prime}}^{T}&\\ {\bf G}^{{\prime}}&&{\bf R}_{{\alpha}}\\ \end{matrix}\right)\left(\begin{matrix}{\bf v}_{{\alpha}}\\ \boldsymbol{\lambda}_{{\alpha}}\end{matrix}\right)=\left(\begin{matrix}{\bf f}^{{\prime}}\\ \boldsymbol{\gamma}^{{\prime}}\end{matrix}\right)$

(0.20)

where

$\displaystyle{\bf M}^{{\prime}}$	$\displaystyle={\bf M}_{{\alpha\alpha}}-{\bf M}_{{\alpha\beta}}{\bf G}_{{\beta\alpha}}-{\bf G}_{{\beta\alpha}}^{T}{\bf M}_{{\beta\alpha}}+{\bf G}_{{\beta\alpha}}^{T}{\bf M}_{{\beta\beta}}{\bf G}_{{\beta\alpha}}$	(0.21)
$\displaystyle{\bf G}^{{\prime}}$	$\displaystyle={\bf G}_{{\alpha\alpha}}-{\bf G}_{{\alpha\beta}}{\bf G}_{{\beta\alpha}}$	(0.22)
$\displaystyle{\bf f}^{{\prime}}$	$\displaystyle={\bf f}_{{\alpha}}-{\bf G}_{{\beta\alpha}}^{T}{\bf f}_{{\beta}}-({\bf M}_{{\alpha\beta}}-{\bf G}_{{\beta\alpha}}^{T}{\bf M}_{{\beta\beta}}){\bf g}_{{\beta}}$	(0.23)
$\displaystyle\boldsymbol{\gamma}^{{\prime}}$	$\displaystyle={\bf g}_{{\alpha}}-{\bf G}_{{\alpha\beta}}{\bf g}_{{\beta}}.$	(0.24)

If ${\bf g}_{{\beta}}=0$ , then the above calculations simplify to ${\bf f}^{{\prime}}={\bf f}_{{\alpha}}-{\bf G}_{{\beta\alpha}}^{T}{\bf f}_{{\beta}}$ and $\boldsymbol{\gamma}^{{\prime}}={\bf g}_{{\alpha}}$ . To see that this is generally the case, we note that typically ${\bf g}_{{\beta}}=h\dot{\bf G}_{{\beta\alpha}}{\bf v}_{{\alpha}}$ . If ${\bf G}_{{\beta\alpha}}$ is constant, then ${\bf g}_{{\beta}}=0$ . Otherwise, when ${\bf G}_{{\beta\alpha}}$ is not constant (as happens in the case of point-frame attachments), we can replace ${\bf G}_{{\beta\alpha}}$ in the main calculation with ${\bf G}_{{\beta\alpha}}+h\dot{\bf G}_{{\beta\alpha}}$ , and so continue to allow ${\bf g}_{{\beta}}=0$ .

The null-space reduction can be accomplished using the matrix operation

${\bf P}{\bf M}{\bf P}^{T}=\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}^{{\prime}}&0\\ 0&0\end{matrix}\right)$

where

${\bf P}\equiv\left(\begin{matrix}{\bf I}&-{\bf G}_{{\beta\alpha}}^{T}\\ 0&0\end{matrix}\right).$

Moreover, this can be achieved on an attachment-by-attachment basis using a series of operations ${\bf P}_{k}{\bf M}{\bf P}_{k}^{T}$ , where

${\bf P}_{k}\equiv\left(\begin{matrix}{\bf I}&0&-{\bf G}_{{k\alpha}}^{T}&0\\ 0&{\bf I}&0&0\\ 0&0&0&0\\ 0&0&0&{\bf I}\end{matrix}\right)$

is the reduction matrix for a particular attachment. It is possible to show that for two attachments $k$ and $j$ , ${\bf P}_{j}{\bf P}_{k}={\bf P}_{k}{\bf P}_{j}$ and so the order of operations does not matter.

0.18 Attaching to an attached component

The analysis of the previous section assumed that attachments are always made to components which are not themselves attached. If this is not the case, then (0.18) is replaced by

${\bf v}_{{k}}+{\bf G}_{{k\alpha}}{\bf v}_{{\alpha}}+{\bf G}_{{k\beta}}{\bf v}_{{\beta}}=0$

and so the identity matrices in (0.19) no longer appear. However, if there are no loops in the attachment relationships (i.e., the attachment structure forms a directed acyclic graph), then the null-space reduction can still be done, one attachment at a time, starting with those to which no other components are attached. To see this, let the attached system be partitioned into active components $\alpha$ , the single component $k$ to which no other components are attached, and the remaining attached components $\beta$ . This leads to the system

$\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}&{\bf M}_{{\alpha\beta}}&{\bf M}_{{\alpha k}}&{\bf G}_{{\alpha\alpha}}^{T}&{\bf G}_{{\beta\alpha}}^{T}&{\bf G}_{{k\alpha}}^{T}\\ {\bf M}_{{\beta\alpha}}&{\bf M}_{{\beta\beta}}&{\bf M}_{{\beta k}}&{\bf G}_{{\alpha\beta}}^{T}&{\bf G}_{{\beta\beta}}^{T}&{\bf G}_{{k\beta}}^{T}\\ {\bf M}_{{k\alpha}}&{\bf M}_{{k\beta}}&{\bf M}_{{kk}}&{\bf G}_{{\alpha k}}^{T}&0&{\bf I}\\ {\bf G}_{{\alpha\alpha}}&{\bf G}_{{\alpha\beta}}&{\bf G}_{{\alpha k}}&{\bf R}_{{\alpha}}&0&0\\ {\bf G}_{{\beta\alpha}}&{\bf G}_{{\beta\beta}}&0&0&0&0\\ {\bf G}_{{k\alpha}}&{\bf G}_{{k\beta}}&{\bf I}&0&0&0\\ \end{matrix}\right)\left(\begin{matrix}{\bf v}_{{\alpha}}\\ {\bf v}_{{\beta}}\\ {\bf v}_{{k}}\\ \boldsymbol{\lambda}_{{\alpha}}\\ \boldsymbol{\lambda}_{{\beta}}\\ \boldsymbol{\lambda}_{{k}}\end{matrix}\right)=\left(\begin{matrix}{\bf f}_{{\alpha}}\\ {\bf f}_{{\beta}}\\ {\bf f}_{{k}}\\ {\bf g}_{{\alpha}}\\ {\bf g}_{{\beta}}\\ {\bf g}_{{k}}\end{matrix}\right)$

(0.25)

which can be reduced to

$\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}^{{\prime}}&{\bf M}_{{\alpha\beta}}^{{\prime}}&{\bf G}_{{\alpha\alpha}}^{{\prime}}^{T}&{\bf G}_{{\beta\alpha}}^{T}\\ {\bf M}_{{\beta\alpha}}^{{\prime}}&{\bf M}_{{\beta\beta}}^{{\prime}}&{\bf G}_{{\alpha\beta}}^{{\prime}}^{T}&{\bf G}_{{\beta\beta}}^{T}\\ {\bf G}_{{\alpha\alpha}}^{{\prime}}&{\bf G}_{{\alpha\beta}}^{{\prime}}&{\bf R}_{{\alpha}}&0\\ {\bf G}_{{\beta\alpha}}&{\bf G}_{{\beta\beta}}&0&0\\ \end{matrix}\right)\left(\begin{matrix}{\bf v}_{{\alpha}}\\ {\bf v}_{{\beta}}\\ \boldsymbol{\lambda}_{{\alpha}}\\ \boldsymbol{\lambda}_{{\beta}}\end{matrix}\right)=\left(\begin{matrix}{\bf f}_{{\alpha}}^{{\prime}}\\ {\bf f}_{{\beta}}^{{\prime}}\\ {\bf g}_{{\alpha}}^{{\prime}}\\ {\bf g}_{{\beta}}\end{matrix}\right)$

(0.26)

where

$\displaystyle{\bf M}_{{\alpha\alpha}}^{{\prime}}$	$\displaystyle={\bf M}_{{\alpha\alpha}}-{\bf M}_{{\alpha k}}{\bf G}_{{k\alpha}}-{\bf G}_{{k\alpha}}^{T}{\bf M}_{{k\alpha}}+{\bf G}_{{k\alpha}}^{T}{\bf M}_{{kk}}{\bf G}_{{k\alpha}}$	(0.27)
$\displaystyle{\bf M}_{{\alpha\beta}}^{{\prime}}$	$\displaystyle={\bf M}_{{\alpha\beta}}-{\bf M}_{{\alpha k}}{\bf G}_{{k\beta}}-{\bf G}_{{k\alpha}}^{T}{\bf M}_{{k\beta}}+{\bf G}_{{k\alpha}}^{T}{\bf M}_{{kk}}{\bf G}_{{k\beta}}$	(0.28)
$\displaystyle{\bf M}_{{\beta\alpha}}^{{\prime}}$	$\displaystyle={\bf M}_{{\beta\alpha}}-{\bf M}_{{\beta k}}{\bf G}_{{k\alpha}}-{\bf G}_{{k\beta}}^{T}{\bf M}_{{k\alpha}}+{\bf G}_{{k\beta}}^{T}{\bf M}_{{kk}}{\bf G}_{{k\alpha}}$	(0.29)
$\displaystyle{\bf M}_{{\beta\beta}}^{{\prime}}$	$\displaystyle={\bf M}_{{\beta\beta}}-{\bf M}_{{\beta k}}{\bf G}_{{k\beta}}-{\bf G}_{{k\beta}}^{T}{\bf M}_{{k\beta}}+{\bf G}_{{k\beta}}^{T}{\bf M}_{{kk}}{\bf G}_{{k\beta}}$	(0.30)
$\displaystyle{\bf G}_{{\alpha\alpha}}^{{\prime}}$	$\displaystyle={\bf G}_{{\alpha\alpha}}-{\bf G}_{{\alpha k}}{\bf G}_{{k\alpha}}$	(0.31)
$\displaystyle{\bf G}_{{\alpha\beta}}^{{\prime}}$	$\displaystyle={\bf G}_{{\alpha\beta}}-{\bf G}_{{\alpha k}}{\bf G}_{{k\beta}}$	(0.32)
$\displaystyle{\bf f}_{{\alpha}}^{{\prime}}$	$\displaystyle={\bf f}_{{\alpha}}-{\bf G}_{{k\alpha}}^{T}{\bf f}_{{k}}-({\bf M}_{{\alpha k}}-{\bf G}_{{k\alpha}}^{T}{\bf M}_{{kk}}){\bf g}_{{k}}$	(0.33)
$\displaystyle{\bf f}_{{\beta}}^{{\prime}}$	$\displaystyle={\bf f}_{{\beta}}-{\bf G}_{{k\beta}}^{T}{\bf f}_{{k}}-({\bf M}_{{\beta k}}-{\bf G}_{{k\beta}}^{T}{\bf M}_{{kk}}){\bf g}_{{k}}$	(0.34)
$\displaystyle{\bf g}_{{\alpha}}^{{\prime}}$	$\displaystyle={\bf g}_{{\alpha}}-{\bf G}_{{\alpha k}}{\bf g}_{{k}}$	(0.35)

As mentioned above, we can often assume that ${\bf g}_{{k}}=0$ .

Now, assuming that the attachments form a directed acyclic graph, order this graph such that for each attached component $k$ , its masters are either unattached or attached only to components $j>k$ . If attachments are then eliminated in this order, we can be assured that each attached component $k$ is not itself attached to by any of the remaining attachments, and so (0.26) can again be placed in the form (0.25) and the elimination can proceed recursively.

If we wish to compute only the forces imparted by the attached components, we can recursively compute ${\bf f}_{{\alpha}}^{{\prime}}$ and ${\bf f}_{{\beta}}^{{\prime}}$ in the prescribed order.

Velocities of the attached components can be determined by traversing the attachments in reverse order, using the formula

${\bf v}_{{k}}=-{\bf G}_{{k\alpha}}{\bf v}_{{\alpha}}-{\bf G}_{{k\beta}}{\bf v}_{{\beta}}$

where ${\bf G}_{{k\beta}}$ only contains entries for components $j>k$ and hence the velocities ${\bf v}_{{\beta}}$ have already been updated.

One important thing to note from (0.35) is that if a component is attached to more than one master, the resulting mass matrix for those masters is no longer block diagonal. For example, if we have a particle of mass $m_{k}$ attached to two particles with masses $m_{1}$ and $m_{2}$ , respectively, via the formula

${\bf v}_{{k}}=w_{1}{\bf v}_{1}+w_{2}{\bf v}_{2},$

then the mass matrix for particles 1 and 2 is transformed from

$\left(\begin{matrix}m_{1}{\bf I}&0\\ 0&m_{2}{\bf I}\\ \end{matrix}\right)$

into

$\left(\begin{matrix}(m_{1}+w_{1}^{2}\, m_{k})\,{\bf I}&w_{1}w_{2}\, m_{k}\,{\bf I}\\ w_{2}w_{1}\, m_{k}\,{\bf I}&(m_{2}+w_{2}^{2}\, m_{k})\,{\bf I}\\ \end{matrix}\right).$

0.19 Adjustments to mass and fictitous forces arising from attachments

When a component $k$ is attached to a master, equations (0.21) and (0.23) adjust the master’s mass and force (including fictitous forces) to account for the attached component’s mass. We will demonstrate this here for the case of a particle with mass $m$ attached to a rigid body at a location ${\bf c}$ with respect to the body frame’s origin.

Let ${\bf v}_{p}$ be the particle’s velocity in world coordinates, $({\bf v},\boldsymbol{\omega})^{T}$ be the body’s spatial velocity in rotated body coordinates, and ${\bf R}_{{BW}}$ be the rotation transform from body to world coordinates. The attachment constraint then takes the form

${\bf v}_{p}={\bf R}_{{BW}}{\bf v}-{\bf R}_{{BW}}{\bf c}\times\boldsymbol{\omega},$

(0.36)

so that, with respect to (0.19),

${\bf G}_{{\beta\alpha}}=\left(\begin{matrix}-{\bf R}_{{BW}}&{\bf R}_{{BW}}[{\bf c}]\end{matrix}\right).$

The change in the effective inertia ${\bf M}$ of the body can then be calculated from (0.21) with ${\bf M}_{{\alpha\beta}}={\bf M}_{{\beta\alpha}}=0$ :

${\bf M}^{{\prime}}={\bf M}+m\,{\bf G}_{{\beta\alpha}}^{T}{\bf G}_{{\beta\alpha}}={\bf M}+\left(\begin{matrix}m{\bf I}&-m\,[{\bf c}]\\ m\,[{\bf c}]&-m\,[{\bf c}][{\bf c}]\end{matrix}\right),$

which corresponds to adding the inertia of a point pass located at ${\bf c}$ with respect to the body’s frame. Applying the Newton-Euler equations to this point inertia, we obtain

$\left(\begin{matrix}m{\bf I}&-m\,[{\bf c}]\\ m\,[{\bf c}]&-m\,[{\bf c}][{\bf c}]\end{matrix}\right)\left(\begin{matrix}{\bf a}\\ \dot{\boldsymbol{\omega}}\end{matrix}\right)=\left(\begin{matrix}{\bf f}\\ \boldsymbol{\tau}\end{matrix}\right)+\left(\begin{matrix}-m\,\boldsymbol{\omega}\times\boldsymbol{\omega}\times{\bf c}\\ m\,\boldsymbol{\omega}\times{\bf c}\times{\bf c}\times\boldsymbol{\omega}\end{matrix}\right),$

where $({\bf a},\dot{\boldsymbol{\omega}})^{T}$ is the acceleration of the body given in rotated body coordinates (with ${\bf a}={\bf R}_{{WB}}{}^{W}\dot{\bf v}=\dot{\bf v}+\boldsymbol{\omega}\times{\bf v}$ ). We therefore expect the attachment to add the fictitous forces given by the rightmost vector in (0.19).

To verify this, we differentiate (0.36) to obtain

	$\displaystyle\dot{\bf v}_{p}$	$\displaystyle={\bf R}_{{BW}}\dot{\bf v}-{\bf R}_{{BW}}\,{\bf c}\times\dot{\boldsymbol{\omega}}+\dot{\bf R}_{{BW}}{\bf v}-\dot{\bf R}_{{BW}}\,{\bf c}\times\boldsymbol{\omega}$
		$\displaystyle={\bf R}_{{BW}}\dot{\bf v}-{\bf R}_{{BW}}\,{\bf c}\times\dot{\boldsymbol{\omega}}+{\bf R}_{{BW}}\,\boldsymbol{\omega}\times{\bf v}-{\bf R}_{{BW}}\,\boldsymbol{\omega}\times{\bf c}\times\boldsymbol{\omega}$
		$\displaystyle=-{\bf G}_{{\beta\alpha}}\,({\bf a},\dot{\boldsymbol{\omega}})^{T}+{\bf g}_{{\beta}},\quad{\bf g}_{{\beta}}={\bf R}_{{BW}}\,\boldsymbol{\omega}\times\boldsymbol{\omega}\times{\bf c}.$

Substituting this into (0.23) with ${\bf f}_{{\beta}}=0$ and ${\bf M}_{{\alpha\beta}}=0$ yields a change in ${\bf f}$ given by

$m\,\left(\begin{matrix}-{\bf R}_{{BW}}^{T}\\ -[{\bf c}]\,{\bf R}_{{BW}}^{T}\end{matrix}\right)\left(\begin{matrix}{\bf R}_{{BW}}\,\boldsymbol{\omega}\times\boldsymbol{\omega}\times{\bf c}\end{matrix}\right)=\left(\begin{matrix}-m\,\boldsymbol{\omega}\times\boldsymbol{\omega}\times{\bf c}\\ m\,\boldsymbol{\omega}\times{\bf c}\times{\bf c}\times\boldsymbol{\omega}\end{matrix}\right),$

where we have used the identity ${\bf a}\times{\bf b}\times{\bf b}\times{\bf a}=-{\bf b}\times{\bf a}\times{\bf a}\times{\bf b}$ . This does in fact equal the fictitous forces as indicted in (0.19).

0.20 Attachments for one-dimensional constraints

Suppose we have a one-dimensional constraint, so that ${\bf G}$ is $1\times n$ . Then after attachment resolution, ${\bf G}^{{\prime}}={\bf G}_{{\alpha\alpha}}-{\bf G}_{{\alpha\beta}}{\bf G}_{{\beta\alpha}}$ in (0.24) will be sparse, except for a number of row vectors ${\bf g}_{j}$ , $j\in\alpha$ , which are $1\times n$ , with $n$ the velocity size of component $j$ .

Now, if we want to apply the constraint ${\bf G}$ in isolation to project a velocity, we have

${\bf v}={\bf v}_{0}+{\bf M}^{{-1}}{\bf G}^{T}({\bf G}{\bf M}^{{-1}}{\bf G}^{T})^{{-1}}(\gamma-{\bf G}{\bf v}_{0})$

which turns into

${\bf v}_{j}={\bf v}_{{j0}}+{\bf M}^{{-1}}_{j}{\bf g}_{j}^{T}(\sum _{{k\in\alpha}}{\bf g}_{k}{\bf M}^{{-1}}_{k}{\bf g}_{k}^{T})^{{-1}}(\gamma-\sum _{{k\in\alpha}}{\bf g}_{k}{\bf v}_{{k0}}).$

Each term ${\bf g}_{k}{\bf M}^{{-1}}_{k}{\bf g}_{k}^{T}$ is the effective inverse inertia in the direction implied by ${\bf g}_{k}$ .

0.21 Reduction of KKT system with parametrically controlled variables

In this section we consider a system in which a set of components $\rho$ is controlled parametrically. Partitioning this system between $\rho$ , the active components $\alpha$ , and the attached components $\beta$ , and letting $\sigma$ denote the union of the active and parametrically controlled components, we obtain

$\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}&{\bf M}_{{\alpha\rho}}&{\bf M}_{{\alpha\beta}}&{\bf G}_{{\sigma\alpha}}^{T}&{\bf G}_{{\beta\alpha}}^{T}\\ {\bf M}_{{\rho\alpha}}&{\bf M}_{{\rho\rho}}&{\bf M}_{{\rho\beta}}&{\bf G}_{{\sigma\rho}}^{T}&{\bf G}_{{\beta\rho}}^{T}\\ {\bf M}_{{\beta\alpha}}&{\bf M}_{{\beta\rho}}&{\bf M}_{{\beta\beta}}&{\bf G}_{{\sigma\beta}}^{T}&{\bf I}\\ {\bf G}_{{\sigma\alpha}}&{\bf G}_{{\sigma\rho}}&{\bf G}_{{\sigma\beta}}&{\bf R}_{{\sigma}}&0\\ {\bf G}_{{\beta\alpha}}&{\bf G}_{{\beta\rho}}&{\bf I}&0&0\\ \end{matrix}\right)\left(\begin{matrix}{\bf v}_{{\alpha}}\\ {\bf v}_{{\rho}}\\ {\bf v}_{{\beta}}\\ \boldsymbol{\lambda}_{{\sigma}}\\ \boldsymbol{\lambda}_{{\beta}}\end{matrix}\right)=\left(\begin{matrix}{\bf f}_{{\alpha}}\\ {\bf f}_{{\rho}}\\ {\bf f}_{{\beta}}\\ {\bf g}_{{\sigma}}\\ {\bf g}_{{\beta}}\end{matrix}\right).$

By eliminating the attachments, we arrive at

$\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}^{{\prime}}&{\bf M}_{{\alpha\rho}}^{{\prime}}&{\bf G}_{{\sigma\alpha}}^{{\prime}}^{T}\\ {\bf M}_{{\rho\alpha}}^{{\prime}}&{\bf M}_{{\rho\rho}}^{{\prime}}&{\bf G}_{{\sigma\rho}}^{{\prime}}^{T}\\ {\bf G}_{{\sigma\alpha}}^{{\prime}}&{\bf G}_{{\sigma\rho}}^{{\prime}}&{\bf R}_{{\sigma}}\end{matrix}\right)\left(\begin{matrix}{\bf v}_{{\alpha}}\\ {\bf v}_{{\rho}}\\ \boldsymbol{\lambda}_{{\sigma}}\end{matrix}\right)=\left(\begin{matrix}{\bf f}_{{\alpha}}^{{\prime}}\\ {\bf f}_{{\rho}}^{{\prime}}\\ {\bf g}_{{\sigma}}^{{\prime}}\end{matrix}\right).$

where

	$\displaystyle{\bf M}_{{\alpha\alpha}}^{{\prime}}$	$\displaystyle={\bf M}_{{\alpha\alpha}}-{\bf M}_{{\alpha\beta}}{\bf G}_{{\beta\alpha}}-{\bf G}_{{\beta\alpha}}^{T}{\bf M}_{{\beta\alpha}}+{\bf G}_{{\beta\alpha}}^{T}{\bf M}_{{\beta\beta}}{\bf G}_{{\beta\alpha}}$
	$\displaystyle{\bf M}_{{\alpha\rho}}^{{\prime}}$	$\displaystyle={\bf M}_{{\alpha\rho}}-{\bf M}_{{\alpha\beta}}{\bf G}_{{\beta\rho}}-{\bf G}_{{\beta\alpha}}^{T}{\bf M}_{{\beta\rho}}+{\bf G}_{{\beta\alpha}}^{T}{\bf M}_{{\beta\beta}}{\bf G}_{{\beta\rho}}$
	$\displaystyle{\bf M}_{{\rho\alpha}}^{{\prime}}$	$\displaystyle={\bf M}_{{\rho\alpha}}-{\bf M}_{{\rho\beta}}{\bf G}_{{\beta\alpha}}-{\bf G}_{{\beta\rho}}^{T}{\bf M}_{{\beta\alpha}}+{\bf G}_{{\beta\rho}}^{T}{\bf M}_{{\beta\beta}}{\bf G}_{{\beta\alpha}}$
	$\displaystyle{\bf M}_{{\rho\rho}}^{{\prime}}$	$\displaystyle={\bf M}_{{\rho\rho}}-{\bf M}_{{\rho\beta}}{\bf G}_{{\beta\rho}}-{\bf G}_{{\beta\rho}}^{T}{\bf M}_{{\beta\rho}}+{\bf G}_{{\beta\rho}}^{T}{\bf M}_{{\beta\beta}}{\bf G}_{{\beta\rho}}$
	$\displaystyle{\bf G}_{{\sigma\alpha}}^{{\prime}}$	$\displaystyle={\bf G}_{{\sigma\alpha}}-{\bf G}_{{\sigma\beta}}{\bf G}_{{\beta\alpha}}$
	$\displaystyle{\bf G}_{{\sigma\rho}}^{{\prime}}$	$\displaystyle={\bf G}_{{\sigma\rho}}-{\bf G}_{{\sigma\beta}}{\bf G}_{{\beta\rho}}$
	$\displaystyle{\bf f}_{{\alpha}}^{{\prime}}$	$\displaystyle={\bf f}_{{\alpha}}-{\bf G}_{{\beta\alpha}}^{T}{\bf f}_{{\beta}}+({\bf M}_{{\alpha\beta}}-{\bf G}_{{\beta\alpha}}^{T}{\bf M}_{{\beta\beta}}){\bf g}_{{\beta}}$
	$\displaystyle{\bf f}_{{\rho}}^{{\prime}}$	$\displaystyle={\bf f}_{{\rho}}-{\bf G}_{{\beta\rho}}^{T}{\bf f}_{{\beta}}+({\bf M}_{{\rho\beta}}-{\bf G}_{{\beta\rho}}^{T}{\bf M}_{{\beta\beta}}){\bf g}_{{\beta}}$
	$\displaystyle{\bf g}_{{\alpha}}^{{\prime}}$	$\displaystyle={\bf g}_{{\alpha}}-{\bf G}_{{\sigma\beta}}{\bf g}_{{\beta}}$

Since ${\bf v}_{{\rho}}$ is given, we can solve for ${\bf f}_{{\rho}}$ as

${\bf f}_{{\rho}}={\bf M}_{{\rho\alpha}}^{{\prime}}{\bf v}_{{\alpha}}+{\bf M}_{{\rho\rho}}^{{\prime}}{\bf v}_{{\rho}}+{\bf G}_{{\sigma\rho}}^{{\prime}}^{T}\boldsymbol{\lambda}_{{\sigma}}+{\bf G}_{{\beta\rho}}^{T}{\bf f}_{{\beta}}-({\bf M}_{{\rho\beta}}-{\bf G}_{{\beta\rho}}^{T}{\bf M}_{{\beta\beta}}){\bf g}_{{\beta}}$

and reduce the rest of the system to

$\left(\begin{matrix}{\bf M}_{{\alpha\alpha}}^{{\prime}}&{\bf G}_{{\alpha\alpha}}^{{\prime}}^{T}\\ {\bf G}_{{\alpha\alpha}}^{{\prime}}&{\bf R}_{{\alpha}}\\ \end{matrix}\right)\left(\begin{matrix}{\bf v}_{{\alpha}}\\ \boldsymbol{\lambda}_{{\sigma}}\end{matrix}\right)=\left(\begin{matrix}{\bf f}_{{\alpha}}^{{\prime}}-{\bf M}_{{\alpha\rho}}^{{\prime}}{\bf v}_{{\rho}}\\ {\bf g}_{{\alpha}}^{{\prime}}-{\bf G}_{{\sigma\rho}}^{{\prime}}{\bf v}_{{\rho}}\end{matrix}\right).$

0.22 Jacobians for Frame Springs

A frame spring provides elastic restoring forces based on the difference in position and orientation between two frames $1$ and $2$ (see Figure 0.2). If these frames are described with respect to world coordinates by ${\bf X}_{{1W}}$ and ${\bf X}_{{2W}}$ , with

${\bf X}_{{1W}}\equiv\left(\begin{matrix}{\bf R}_{{1W}}&{\bf x}_{1}\\ 0&1\end{matrix}\right)\quad\text{and}\quad{\bf X}_{{2W}}\equiv\left(\begin{matrix}{\bf R}_{{2W}}&{\bf x}_{2}\\ 0&1\end{matrix}\right),$

then the displacement between the two is given by

${\bf X}_{{1W}}^{{-1}}{\bf X}_{{2W}}=\left(\begin{matrix}{\bf R}_{{21}}&{\bf R}_{{1W}}^{T}{\bf x}_{2}-{\bf R}_{{1W}}^{T}{\bf x}_{1}\\ 0&1\end{matrix}\right).$

Here ${\bf R}_{{21}}={\bf R}_{{1W}}^{T}{\bf R}_{{2W}}$ is the rotational transformation from frame $2$ to frame $1$ ; in the discussion below, ${\bf R}_{{XY}}$ will in general denote a rotation from frame $X$ to frame $Y$ .

Figure 0.2: A frame spring between two frames $1$ and $2$ , which are rigidly connected to frames $A$ and $B$ .

If ${\bf u}$ and $\phi$ are the rotation axis and angle associated with ${\bf R}_{{21}}$ , and $k_{t}$ and $k_{r}$ are translational and rotational stiffnesses, then the restoring forces and torques felt respectively in frames $1$ and $2$ are given by

$\displaystyle{\bf f}_{{1k}}$	$\displaystyle=k_{t}{\bf R}_{{1W}}^{T}({\bf x}_{2}-{\bf x}_{1}),$
$\displaystyle\boldsymbol{\tau}_{{1k}}$	$\displaystyle=k_{r}(\phi{\bf u}),$
$\displaystyle{\bf f}_{{2k}}$	$\displaystyle=-k_{t}{\bf R}_{{2W}}^{T}({\bf x}_{2}-{\bf x}_{1}),$
$\displaystyle\boldsymbol{\tau}_{{2k}}$	$\displaystyle=-k_{r}{\bf R}_{{12}}(\phi{\bf u}).$	(0.37)

In addition, if $\boldsymbol{\omega}_{1}$ and $\boldsymbol{\omega}_{2}$ are the angular velocities of frames $1$ and $2$ (expressed in world coordinates), we can also define damping forces according to

$\displaystyle{\bf f}_{{1d}}$	$\displaystyle=d_{t}{\bf R}_{{1W}}^{T}(\dot{\bf x}_{2}-\dot{\bf x}_{1}),$
$\displaystyle\boldsymbol{\tau}_{{1d}}$	$\displaystyle=d_{r}{\bf R}_{{1W}}^{T}(\boldsymbol{\omega}_{2}-\boldsymbol{\omega}_{1}),$
$\displaystyle{\bf f}_{{2d}}$	$\displaystyle=-d_{t}{\bf R}_{{2W}}^{T}(\dot{\bf x}_{2}-\dot{\bf x}_{1}),$
$\displaystyle\boldsymbol{\tau}_{{2d}}$	$\displaystyle=-d_{r}{\bf R}_{{2W}}^{T}(\boldsymbol{\omega}_{2}-\boldsymbol{\omega}_{1}).$	(0.38)

In practice, frames $1$ and $2$ are rigidly connected to a secondary pair of frames $A$ and $B$ , whose transformations to world coordinates are given by

${\bf X}_{{AW}}=\left(\begin{matrix}{\bf R}_{{AW}}&{\bf x}_{A}\\ 0&1\end{matrix}\right)\quad\text{and}\quad{\bf X}_{{BW}}=\left(\begin{matrix}{\bf R}_{{BW}}&{\bf x}_{B}\\ 0&1\end{matrix}\right).$

Frames $A$ and $B$ are typically the coordinate frames of rigid bodies and are the entities associated with dynamic calculations, and so we need to determine forces and Jacobians with respect to these. The rigid transformations from $1$ and $2$ to $A$ and $B$ are given by ${\bf X}_{{1A}}$ and ${\bf X}_{{2B}}$ , where

${\bf X}_{{1A}}=\left(\begin{matrix}{\bf R}_{{1A}}&{\bf p}_{1}\\ 0&1\end{matrix}\right)\quad\text{and}\quad{\bf X}_{{2B}}=\left(\begin{matrix}{\bf R}_{{2B}}&{\bf p}_{2}\\ 0&1\end{matrix}\right).$

Mapping (0.37) and (0.38) into $A$ and $B$ yields

$\displaystyle{\bf f}_{{Ak}}$	$\displaystyle=k_{t}{\bf R}_{{AW}}^{T}({\bf x}_{2}-{\bf x}_{1}),$
$\displaystyle\boldsymbol{\tau}_{{Ak}}$	$\displaystyle=k_{r}{\bf R}_{{1A}}(\phi{\bf u})+{\bf p}_{1}\times{\bf f}_{{Ak}},$
$\displaystyle{\bf f}_{{Bk}}$	$\displaystyle=-k_{t}{\bf R}_{{BW}}^{T}({\bf x}_{2}-{\bf x}_{1}),$
$\displaystyle\boldsymbol{\tau}_{{Bk}}$	$\displaystyle=-k_{r}{\bf R}_{{1B}}(\phi{\bf u})+{\bf p}_{2}\times{\bf f}_{{Bk}}.$	(0.39)

and

$\displaystyle{\bf f}_{{Ad}}$	$\displaystyle=d_{t}{\bf R}_{{AW}}^{T}(\dot{\bf x}_{2}-\dot{\bf x}_{1}),$
$\displaystyle\boldsymbol{\tau}_{{Ad}}$	$\displaystyle=d_{r}{\bf R}_{{AW}}^{T}(\boldsymbol{\omega}_{2}-\boldsymbol{\omega}_{1})+{\bf p}_{1}\times{\bf f}_{{Ad}},$
$\displaystyle{\bf f}_{{Bd}}$	$\displaystyle=-d_{t}{\bf R}_{{BW}}^{T}(\dot{\bf x}_{2}-\dot{\bf x}_{1}),$
$\displaystyle\boldsymbol{\tau}_{{Bd}}$	$\displaystyle=-d_{r}{\bf R}_{{BW}}^{T}(\boldsymbol{\omega}_{2}-\boldsymbol{\omega}_{1})+{\bf p}_{2}\times{\bf f}_{{Bd}}$	(0.40)

If ${\bf v}_{A}$ , ${\bf v}_{B}$ , $\boldsymbol{\omega}_{A}$ and $\boldsymbol{\omega}_{B}$ are the translational and rotational velocities of $A$ and $B$ (in world coordinates), and ${}^{W}{\bf p}_{1}$ and ${}^{W}{\bf p}_{2}$ are the world coordinate representations of ${\bf p}_{1}$ and ${\bf p}_{2}$ , the the translational velocities $\dot{\bf x}_{1}$ and $\dot{\bf x}_{2}$ are described by

$\dot{\bf x}_{1}={\bf v}_{A}+\boldsymbol{\omega}_{A}\times{}^{W}{\bf p}_{1}\quad\text{and}\quad\dot{\bf x}_{2}={\bf v}_{B}+\boldsymbol{\omega}_{B}\times{}^{W}{\bf p}_{2}$

(0.41)

while the rotational velocities satisfy

$\boldsymbol{\omega}_{1}=\boldsymbol{\omega}_{A}\quad\text{and}\quad\boldsymbol{\omega}_{2}=\boldsymbol{\omega}_{B}.$

(0.42)

0.22.1 Position Jacobian

In this section we develop the position Jacobian, which describes the variations in forces arising from variations $\delta{\bf x}_{A}$ , $\delta{\bf x}_{B}$ , $\delta\boldsymbol{\theta}_{A}$ , and $\delta\boldsymbol{\theta}_{B}$ in the position and orientation of $A$ and $B$ . These will nominally be given in the coordinates of the associated frame; otherwise, the coordinate frame will be indicated. Conversion of these quantities between frames is given by an appropriate rotational transform: ${}^{W}\delta{\bf x}_{A}={\bf R}_{{AW}}\delta{\bf x}_{A}$ , ${}^{W}\delta\boldsymbol{\theta}_{A}={\bf R}_{{AW}}\delta\boldsymbol{\theta}_{A}$ , etc.

We begin with frame $A$ and the stiffness force ${\bf f}_{{Ak}}$ described by (0.39). Because of the rigid coupling between $1$ and $2$ and $A$ and $B$ , the variations of ${\bf x}_{1}$ and ${\bf x}_{2}$ depend on both the translational and rotational variations in $A$ and $B$ :

$\delta{\bf x}_{1}={}^{W}\delta{\bf x}_{A}-{}^{W}{\bf p}_{1}\times\delta{}^{W}\boldsymbol{\theta}_{A}\quad\text{and}\quad\delta{\bf x}_{2}={}^{W}\delta{\bf x}_{B}-{}^{W}{\bf p}_{2}\times\delta{}^{W}\boldsymbol{\theta}_{B}$

Also, since ${\bf x}_{1}$ and ${\bf x}_{2}$ are given in world coordinates, when frame $1$ rotates, the resulting ${\bf f}_{{1k}}$ will be changed by ${\bf f}_{{1k}}\times\delta{}^{1}\boldsymbol{\theta}_{A}$ . With respect to $A$ , this variation is described by ${}^{A}{\bf f}_{{1k}}\times\boldsymbol{\theta}_{A}$ , where ${}^{A}{\bf f}_{{1k}}\equiv{\bf R}_{{1A}}{\bf f}_{{1k}}$ . Putting all this together, we find that the variation of ${\bf f}_{{Ak}}$ is given by

	$\displaystyle\delta{\bf f}_{{Ak}}$	$\displaystyle=k_{t}{\bf R}_{{AW}}^{T}(\delta{\bf x}_{2}-\delta{\bf x}_{1})+{}^{A}{\bf f}_{{1k}}\times\delta\boldsymbol{\theta}_{A}$
		$\displaystyle=k_{t}(-\delta{\bf x}_{A}+{\bf p}_{1}\times\boldsymbol{\omega}_{A}+{\bf R}_{{BA}}\delta{\bf x}_{B}-{\bf R}_{{BA}}{\bf p}_{2}\times\boldsymbol{\omega}_{B})+{}^{A}{\bf f}_{{1k}}\times\delta\boldsymbol{\theta}_{A}.$		(0.43)

The damping force ${\bf f}_{{Ad}}$ of (0.40) also varies with position, because the velocities $\dot{\bf x}_{1}$ and $\dot{\bf x}_{2}$ depend on the orientations of $A$ and $B$ . Specifically, from (0.41), we have

$\dot{\bf x}_{1}={}^{W}{\bf v}_{A}+{}^{W}\boldsymbol{\omega}_{A}\times{}^{W}{\bf p}_{1}$

which varies with orientation because ${}^{W}{\bf p}_{1}$ does. In particular, ${}^{W}{\bf p}_{1}={\bf R}_{{AW}}{\bf p}_{1}$ , and so

${}^{W}\delta{\bf p}_{1}=\delta{\bf R}_{{AW}}{\bf p}_{1}={\bf R}_{{AW}}[\delta\boldsymbol{\theta}_{{A}}]{\bf p}_{1}=-{\bf R}_{{AW}}[{\bf p}_{1}]\delta\boldsymbol{\theta}_{A}.$

Similarly,

$\dot{\bf x}_{2}={}^{W}{\bf v}_{B}+{}^{W}\boldsymbol{\omega}_{B}\times{}^{W}{\bf p}_{2}$

and

${}^{W}\delta{\bf p}_{2}=-{\bf R}_{{BW}}[{\bf p}_{2}]\delta\boldsymbol{\theta}_{B}$

Adding in the rotational term ${\bf f}_{{Ad}}\times\delta\boldsymbol{\omega}_{A}$ , the positional variation in ${\bf f}_{{Ad}}$ is then

	$\displaystyle\delta{\bf f}_{{Ad}}$	$\displaystyle=d_{t}{\bf R}_{{AW}}^{T}(-\boldsymbol{\omega}_{B}\times({\bf R}_{{BW}}[{\bf p}_{2}]\delta\boldsymbol{\theta}_{B})+\boldsymbol{\omega}_{A}\times({\bf R}_{{AW}}[{\bf p}_{1}]\delta\boldsymbol{\theta}_{A}))+{\bf f}_{{Ad}}\times\delta\boldsymbol{\omega}_{A}$
		$\displaystyle=-d_{t}{\bf R}_{{BA}}[{}^{B}\boldsymbol{\omega}_{B}][{\bf p}_{2}]\delta\boldsymbol{\theta}_{B}+d_{t}[{}^{A}\boldsymbol{\omega}_{A}][{\bf p}_{1}]\delta\boldsymbol{\theta}_{A}+{\bf f}_{{Ad}}\times\delta\boldsymbol{\omega}_{A}$		(0.44)

To determine the variation of $\boldsymbol{\tau}_{{Ak}}$ , we need to determine the variation of $\phi{\bf u}$ . This can be done by finding ${\bf d}(\phi{\bf u})/dt$ in terms of the angular velocity $\boldsymbol{\omega}$ of frame $2$ with respect to frame $1$ , as represented in frame $1$ . We begin by finding the derivative of a quaternion ${\bf q}=(\cos(\phi/2),\sin(\phi/2){\bf u})$ which gives the orientation of $2$ with respect to $1$ . Differentiating each term and equating it with the corresponding term in (0.1) yields

$\frac{d\cos(\phi/2){\bf u}}{dt}=-\frac{1}{2}\sin(\phi/2)\dot{\phi}=-\frac{1}{2}\sin(\phi/2)\boldsymbol{\omega}\cdot{\bf u}$

$\frac{d\sin(\phi/2){\bf u}}{dt}=\frac{1}{2}\cos(\phi/2)\dot{\phi}{\bf u}+\sin(\phi/2)\dot{\bf u}=\frac{1}{2}(\cos(\phi/2)\boldsymbol{\omega}+\sin(\phi/2)\boldsymbol{\omega}\times{\bf u}).$

From this we obtain

$\dot{\phi}=\boldsymbol{\omega}\cdot{\bf u}$

and

$\dot{\bf u}=\frac{1}{2}(\cot(\phi/2)\boldsymbol{\omega}-\cot(\phi/2)(\boldsymbol{\omega}\cdot{\bf u}){\bf u}+\boldsymbol{\omega}\times{\bf u})$

Then:

$\frac{d(\phi{\bf u})}{dt}=\dot{\phi}{\bf u}+\phi\dot{\bf u}=(\boldsymbol{\omega}\cdot{\bf u}){\bf u}+\frac{\phi}{2}\left(\cot(\phi/2)(\boldsymbol{\omega}-(\boldsymbol{\omega}\cdot{\bf u}){\bf u})+\boldsymbol{\omega}\times{\bf u}\right)$

which can be rearranged into

$\frac{d(\phi{\bf u})}{dt}={\bf U}\boldsymbol{\omega},\quad{\bf U}\equiv\frac{1}{2}\left(\phi\cot(\phi/2){\bf I}+(2-\phi\cot(\phi/2)){\bf u}{\bf u}^{T}-\phi[{\bf u}]\right)$

(0.45)

which matches the results obtained by Sebastian Grassia in A Practical Parameterization of 2 and 3 Degree of Freedom Rotations. (In that paper, $\phi{\bf u}$ is represented by a vector ${\bf v}$ , $\dot{\bf v}$ is given in terms of ${\bf v}$ instead of ${\bf u}$ , and $\boldsymbol{\omega}$ is specified using the rotated frame (i.e., frame $2$ ) instead of the base frame.) In the case where $\phi=0$ , we can use

$\lim _{{\phi\rightarrow 0}}\phi\cot(\phi/2)=2$

to obtain ${\bf U}={\bf I}$ .

In the above analysis, ${\bf u}$ and $\boldsymbol{\omega}$ represent the rotation and angular velocity of frame 2 with respect to 1, represented in frame 1, so that $\boldsymbol{\omega}\equiv{}^{1}\boldsymbol{\omega}_{{21}}$ . For variations in orientation, $\boldsymbol{\omega}$ is replaced by $\delta\boldsymbol{\theta}_{{21}}$ . Noting that $\delta\boldsymbol{\theta}_{{21}}=\delta\boldsymbol{\theta}_{2}-\delta\boldsymbol{\theta}_{1}$ , ${}^{1}\delta\boldsymbol{\theta}_{1}={\bf R}_{{A1}}\delta\boldsymbol{\theta}_{A}$ , and ${}^{2}\delta\boldsymbol{\theta}_{2}={\bf R}_{{B2}}\delta\boldsymbol{\theta}_{B}$ , we obtain

$\delta\boldsymbol{\tau}_{{Ak}}=-k_{r}{\bf R}_{{1A}}{\bf U}{\bf R}_{{A1}}\delta\boldsymbol{\theta}_{A}+k_{r}{\bf R}_{{1A}}{\bf U}{\bf R}_{{B1}}\delta\boldsymbol{\theta}_{B}+{\bf p}_{1}\times\delta{\bf f}_{{Ak}}$

(0.46)

In this expression there is no rotational term involving $\boldsymbol{\tau}_{{1k}}\times\delta\boldsymbol{\theta}_{A}$ term. That is because the derivative of $\phi{\bf u}$ expressed by ${\bf U}$ is already with respect to the rotating frame. However, it is convenient to incorporate a $\boldsymbol{\tau}_{{1k}}\times\delta\boldsymbol{\theta}_{A}$ term for consistency with other components of the Jacobian. Letting ${}^{A}\boldsymbol{\tau}_{{1k}}\equiv{\bf R}_{{1A}}\boldsymbol{\tau}_{{1k}}$ , it is easy to verify that

$-k_{r}{\bf R}_{{1A}}({\bf U}-{\bf U}^{T}){\bf R}_{{A1}}\delta\boldsymbol{\theta}_{A}=k_{r}{\bf R}_{{1A}}(\phi[{\bf u}]){\bf R}_{{A1}}\delta\boldsymbol{\theta}_{A}={}^{A}\boldsymbol{\tau}_{{1k}}\times\delta\boldsymbol{\theta}_{A}$

since

${\bf U}^{T}\equiv\frac{1}{2}\left(\phi\cot(\phi/2){\bf I}+(2-\phi\cot(\phi/2)){\bf u}{\bf u}^{T}+\phi[{\bf u}]\right).$

(0.47)

differs from ${\bf U}$ only in the sign of the $\phi[{\bf u}]$ term. This allows us to reformulate (0.46) as

$\delta\boldsymbol{\tau}_{{Ak}}=-k_{r}{\bf R}_{{1A}}{\bf U}^{T}{\bf R}_{{A1}}\delta\boldsymbol{\theta}_{A}+{}^{A}\boldsymbol{\tau}_{{1k}}\times\delta\boldsymbol{\theta}_{A}+k_{r}{\bf R}_{{1A}}{\bf U}{\bf R}_{{B1}}\delta\boldsymbol{\theta}_{B}+{\bf p}_{1}\times\delta{\bf f}_{{Ak}}.$

(0.48)

The positional variation of $\boldsymbol{\tau}_{{Ad}}$ is due only to rotational effects and the dependence on ${\bf f}_{{Ad}}$ :

$\delta\boldsymbol{\tau}_{{Ad}}={}^{A}\boldsymbol{\tau}_{{1d}}\times\delta\boldsymbol{\theta}_{A}+{\bf p}_{1}\times\delta{\bf f}_{{Ad}}$

(0.49)

Turning our attention to frame $B$ , and ${\bf f}_{{Bk}}$ , $\boldsymbol{\tau}_{{Bk}}$ , and ${\bf f}_{{Bd}}$ from (0.39) and (0.40), it is easy to show that

$\delta{\bf f}_{{Bk}}=k_{t}(-\delta{\bf x}_{B}+{\bf p}_{2}\times\delta\boldsymbol{\theta}_{B}+{\bf R}_{{AB}}\delta{\bf x}_{A}-{\bf R}_{{AB}}{\bf p}_{1}\times\delta\boldsymbol{\theta}_{A})+{}^{B}{\bf f}_{{2k}}\times\delta\boldsymbol{\theta}_{B}.$

(0.50)

and

$\delta{\bf f}_{{Bd}}=d_{t}[\boldsymbol{\omega}_{B}][{\bf p}_{2}]\delta\boldsymbol{\theta}_{B}-d_{t}{\bf R}_{{AB}}[\boldsymbol{\omega}_{A}][{\bf p}_{1}]\delta\boldsymbol{\theta}_{A}+{}^{B}{\bf f}_{{2d}}\times\delta\boldsymbol{\theta}_{B}.$

(0.51)

For $\boldsymbol{\tau}_{{Bk}}$ , we need to take the derivative of ${\bf R}_{{1B}}(\phi{\bf u})$ :

$\frac{d({\bf R}_{{1B}}(\phi{\bf u}))}{dt}={\bf R}_{{1B}}\frac{d(\phi{\bf u})}{dt}+\dot{\bf R}_{{1B}}(\phi{\bf u})={\bf R}_{{1B}}{\bf U}\boldsymbol{\omega}+\dot{\bf R}_{{1B}}(\phi{\bf u}).$

where again $\boldsymbol{\omega}\equiv{}^{1}\boldsymbol{\omega}_{{21}}$ . Since ${\bf R}_{{1B}}={\bf R}_{{2B}}{\bf R}_{{12}}$ and ${\bf R}_{{2B}}$ is constant, we have

$\dot{\bf R}_{{1B}}={\bf R}_{{2B}}{\bf R}_{{12}}[\boldsymbol{\omega}]=-{\bf R}_{{1B}}[\boldsymbol{\omega}]$

and therefore

$\frac{d({\bf R}_{{1B}}(\phi{\bf u}))}{dt}={\bf R}_{{1B}}{\bf U}\boldsymbol{\omega}-{\bf R}_{{1B}}[\boldsymbol{\omega}](\phi{\bf u})={\bf R}_{{1B}}{\bf U}\boldsymbol{\omega}+{\bf R}_{{1B}}[\phi{\bf u}]\boldsymbol{\omega}={\bf R}_{{1B}}{\bf U}^{T}\boldsymbol{\omega}.$

Replacing $\boldsymbol{\omega}$ with ${}^{1}\delta\boldsymbol{\theta}_{B}-\delta\boldsymbol{\theta}_{A}$ , we can obtain

$\delta\boldsymbol{\tau}_{{Bk}}=-k_{r}{\bf R}_{{1B}}{\bf U}^{T}{\bf R}_{{A1}}\delta\boldsymbol{\theta}_{A}+k_{r}{\bf R}_{{1B}}{\bf U}^{T}{\bf R}_{{B1}}\delta\boldsymbol{\theta}_{B}+{\bf p}_{2}\times\delta{\bf f}_{{Bk}}$

which we can again be rearranged to contain a $\boldsymbol{\tau}_{B}\times\delta\boldsymbol{\theta}_{B}$ term:

$\delta\boldsymbol{\tau}_{{Bk}}=-k_{r}{\bf R}_{{1B}}{\bf U}^{T}{\bf R}_{{A1}}\delta\boldsymbol{\theta}_{A}+k_{r}{\bf R}_{{1B}}{\bf U}{\bf R}_{{B1}}\delta\boldsymbol{\theta}_{B}+\boldsymbol{\tau}_{B}\times\delta\boldsymbol{\theta}_{B}+{\bf p}_{2}\times\delta{\bf f}_{{Bk}}.$

(0.52)

Finally, we have

$\delta\boldsymbol{\tau}_{{Bd}}={}^{B}\boldsymbol{\tau}_{{1d}}\times\delta\boldsymbol{\theta}_{B}+{\bf p}_{2}\times\delta{\bf f}_{{Bd}}.$

(0.53)

Equations (0.43), (0.44), (0.48), (0.49), (0.50), (0.51), (0.52) and (0.53) can be combined to form the positional Jacobian ${\bf J}_{p}$ which satisfies

$\left(\begin{matrix}\delta{\bf f}_{A}\\ \delta\boldsymbol{\tau}_{A}\\ \delta{\bf f}_{B}\\ \delta\boldsymbol{\tau}_{B}\end{matrix}\right)={\bf J}_{{p}}\left(\begin{matrix}\delta{\bf x}_{A}\\ \delta\boldsymbol{\theta}_{A}\\ \delta{\bf x}_{B}\\ \delta\boldsymbol{\theta}_{B}\end{matrix}\right).$

where we define ${}^{A}{\bf f}_{1}\equiv{}^{A}{\bf f}_{{1k}}+{}^{A}{\bf f}_{{1d}}$ , ${}^{A}\boldsymbol{\tau}_{1}\equiv{}^{A}\boldsymbol{\tau}_{{1k}}+{}^{A}\boldsymbol{\tau}_{{1d}}$ , ${}^{B}{\bf f}_{2}\equiv{}^{B}{\bf f}_{{2k}}+{}^{B}{\bf f}_{{2d}}$ , and ${}^{B}\boldsymbol{\tau}_{2}\equiv{}^{B}\boldsymbol{\tau}_{{2k}}+{}^{B}\boldsymbol{\tau}_{{2d}}$ . Letting ${\bf J}_{{ij}}$ denote the $i,j$ -th $3\times 3$ block of ${\bf J}_{p}$ , ${\bf J}_{p}$ is then given by

$\left(\begin{matrix}-k_{t}{\bf I}&k_{t}[{\bf p}_{1}]+d_{t}[\boldsymbol{\omega}_{A}][{\bf p}_{1}]+[{}^{A}{\bf f}_{1}]&k_{t}{\bf R}_{{BA}}&-k_{t}{\bf R}_{{BA}}[{\bf p}_{2}]-d_{t}{\bf R}_{{BA}}[\boldsymbol{\omega}_{B}][{\bf p}_{2}]\\ -k_{t}[{\bf p}_{1}]&[{\bf p}_{1}]{\bf J}_{{01}}-k_{r}{\bf R}_{{1A}}{\bf U}^{T}{\bf R}_{{A1}}+[{}^{A}\boldsymbol{\tau}_{1}]&k_{t}[{\bf p}_{1}]{\bf R}_{{BA}}&[{\bf p}_{1}]{\bf J}_{{03}}+k_{r}{\bf R}_{{1A}}{\bf U}{\bf R}_{{B1}}\\ k_{t}{\bf R}_{{AB}}&-k_{t}{\bf R}_{{AB}}[{\bf p}_{1}]-d_{t}{\bf R}_{{AB}}[\boldsymbol{\omega}_{A}][{\bf p}_{1}]&-k_{t}{\bf I}&k_{t}[{\bf p}_{2}]+d_{t}[\boldsymbol{\omega}_{B}][{\bf p}_{2}]+[{}^{B}{\bf f}_{2}]\\ k_{t}[{\bf p}_{2}]{\bf R}_{{AB}}&[{\bf p}_{2}]{\bf J}_{{21}}+k_{r}{\bf R}_{{1B}}{\bf U}^{T}{\bf R}_{{A1}}&-k_{t}[{\bf p}_{2}]&[{\bf p}_{2}]{\bf J}_{{23}}-k_{r}{\bf R}_{{1B}}{\bf U}{\bf R}_{{B1}}+[{}^{B}\boldsymbol{\tau}_{2}]\end{matrix}\right).$

The inputs and outputs for the above matrix are given with respect to local frame coordinates, and the quantities within the matrix itself are given with respect to either frame $A$ or $B$ , depending on the sub-block.

0.22.2 Velocity Jacobian

The velocity Jacobian describes the variation in damping forces (0.40) arising from variations $\delta{\bf v}_{A}$ , $\delta{\bf v}_{B}$ , $\delta\boldsymbol{\omega}_{A}$ , and $\delta\boldsymbol{\omega}_{B}$ in the translational and rotational velocities of $A$ and $B$ . Starting with ${\bf f}_{{Ad}}$ and $\boldsymbol{\tau}_{{Ad}}$ in (0.40), and using (0.41) and (0.42), it is easy to show that

	$\displaystyle\delta{\bf f}_{{Ad}}$	$\displaystyle=d_{t}{\bf R}_{{AW}}^{T}(\delta{\bf v}_{B}-{}^{W}{\bf p}_{2}\times\delta\boldsymbol{\omega}_{B}-\delta{\bf v}_{A}+{}^{W}{\bf p}_{1}\times\delta\boldsymbol{\omega}_{A})$
		$\displaystyle=d_{t}{\bf R}_{{BA}}(\delta{}^{B}{\bf v}_{B}-[{\bf p}_{2}]\delta{}^{B}\boldsymbol{\omega}_{B})-d_{t}(\delta{}^{A}{\bf v}_{A}-[{\bf p}_{1}]\delta{}^{A}\boldsymbol{\omega}_{A})$		(0.54)

and

	$\displaystyle\delta\boldsymbol{\tau}_{{Ad}}$	$\displaystyle=d_{r}{\bf R}_{{AW}}^{T}(\delta\boldsymbol{\omega}_{B}-\delta\boldsymbol{\omega}_{A})+{\bf p}_{1}\times\delta{\bf f}_{{Ad}}$
		$\displaystyle=d_{r}{\bf R}_{{BA}}{}^{B}\delta\boldsymbol{\omega}_{B}-d_{r}\delta{}^{A}\boldsymbol{\omega}_{A}+{\bf p}_{1}\times\delta{\bf f}_{{Ad}}.$		(0.55)

Likewise, for ${\bf f}_{{Bd}}$ and $\boldsymbol{\tau}_{{Bd}}$ , we have

$\delta{\bf f}_{{Bd}}=-d_{t}(\delta{}^{B}{\bf v}_{B}-[{\bf p}_{2}]\delta{}^{B}\boldsymbol{\omega}_{B})+d_{t}{\bf R}_{{AB}}(\delta{}^{A}{\bf v}_{A}-[{\bf p}_{1}]\delta{}^{A}\boldsymbol{\omega}_{A})$

(0.56)

and

$\delta\boldsymbol{\tau}_{{Bd}}=-d_{r}{}^{B}\delta\boldsymbol{\omega}_{B}-d_{r}{\bf R}_{{AB}}\delta{}^{A}\boldsymbol{\omega}_{A}+{\bf p}_{2}\times\delta{\bf f}_{{Bd}}.$

(0.57)

Assembling (0.54), (0.55), (0.56) and (0.57) yields the velocity Jacobian ${\bf J}_{v}$ , which satisfies

$\left(\begin{matrix}\delta{\bf f}_{A}\\ \delta\boldsymbol{\tau}_{A}\\ \delta{\bf f}_{B}\\ \delta\boldsymbol{\tau}_{B}\end{matrix}\right)={\bf J}_{{v}}\left(\begin{matrix}\delta{\bf v}_{A}\\ \delta\boldsymbol{\omega}_{A}\\ \delta{\bf v}_{B}\\ \delta\boldsymbol{\omega}_{B}\end{matrix}\right)$

with ${\bf J}_{v}$ given by

$\left(\begin{matrix}-d_{t}{\bf I}&d_{t}[{\bf p}_{1}]&d_{t}{\bf R}_{{BA}}&-d_{t}{\bf R}_{{BA}}[{\bf p}_{2}]\\ -d_{t}[{\bf p}_{1}]&-d_{r}{\bf I}+d_{t}[{\bf p}_{1}][{\bf p}_{1}]&d_{t}[{\bf p}_{1}]{\bf R}_{{BA}}&d_{r}{\bf R}_{{BA}}-d_{t}[{\bf p}_{1}]{\bf R}_{{BA}}[{\bf p}_{2}]\\ d_{t}{\bf R}_{{AB}}&-d_{t}{\bf R}_{{AB}}[{\bf p}_{1}]&-d_{t}{\bf I}&d_{t}[{\bf p}_{2}]\\ d_{t}[{\bf p}_{2}]{\bf R}_{{AB}}&d_{r}{\bf R}_{{AB}}-d_{t}[{\bf p}_{2}]{\bf R}_{{AB}}[{\bf p}_{1}]&-d_{t}[{\bf p}_{2}]&-d_{r}{\bf I}+d_{t}[{\bf p}_{2}][{\bf p}_{2}]\\ \end{matrix}\right).$

0.22.3 Jacobians in world coordinates

The Jacobians derived in the previous two sections are each expressed with respect to body coordinates. In cases where the body velocities are expressed in world coordinates, the Jacobians must be adjusted to reflect this.

Beginning with the velocity Jacobian, the corresponding matrix ${}^{W}{\bf J}_{v}$ in which inputs and outputs are given with world coordinates is obtained from

${}^{W}{\bf J}_{v}=\left(\begin{matrix}{\bf R}_{{AW}}&0&0&0\\ 0&{\bf R}_{{AW}}&0&0\\ 0&0&{\bf R}_{{BW}}&0\\ 0&0&0&{\bf R}_{{BW}}\end{matrix}\right){\bf J}_{v}\left(\begin{matrix}{\bf R}_{{AW}}^{T}&0&0&0\\ 0&{\bf R}_{{AW}}^{T}&0&0\\ 0&0&{\bf R}_{{BW}}^{T}&0\\ 0&0&0&{\bf R}_{{BW}}^{T}\end{matrix}\right).$

(0.58)

Making use of the identity

${\bf R}[{\bf a}]=[{\bf R}{\bf a}]{\bf R},$

where ${\bf R}$ is a rotation, we then obtain

${}^{W}{\bf J}_{v}=\left(\begin{matrix}-d_{t}{\bf I}&d_{t}[{\bf p}_{1}]&d_{t}{\bf I}&-d_{t}[{\bf p}_{2}]\\ -d_{t}[{\bf p}_{1}]&-d_{r}{\bf I}+d_{t}[{\bf p}_{1}][{\bf p}_{1}]&d_{t}[{\bf p}_{1}]&d_{r}{\bf I}-d_{t}[{\bf p}_{1}][{\bf p}_{2}]\\ d_{t}{\bf I}&-d_{t}[{\bf p}_{1}]&-d_{t}{\bf I}&d_{t}[{\bf p}_{2}]\\ d_{t}[{\bf p}_{2}]&d_{r}{\bf I}-d_{t}[{\bf p}_{2}][{\bf p}_{1}]&-d_{t}[{\bf p}_{2}]&-d_{r}{\bf I}+d_{t}[{\bf p}_{2}][{\bf p}_{2}]\\ \end{matrix}\right),$

with all quantities expressed in world coordinates.

Applying the same formulation to the position Jacobian ${\bf J}_{p}$ yields the result

$\left(\begin{matrix}-k_{t}{\bf I}&k_{t}[{\bf p}_{1}]+d_{t}[\boldsymbol{\omega}_{A}][{\bf p}_{1}]+[{}^{W}{\bf f}_{1}]&k_{t}{\bf I}&-k_{t}[{\bf p}_{2}]-d_{t}[\boldsymbol{\omega}_{B}][{\bf p}_{2}]\\ -k_{t}[{\bf p}_{1}]&[{\bf p}_{1}]{\bf J}_{{01}}-k_{r}{}^{W}{\bf U}^{T}+[{}^{W}\boldsymbol{\tau}_{1}]&k_{t}[{\bf p}_{1}]&[{\bf p}_{1}]{\bf J}_{{03}}+k_{r}{}^{W}{\bf U}\\ k_{t}{\bf I}&-k_{t}[{\bf p}_{1}]-d_{t}[\boldsymbol{\omega}_{A}][{\bf p}_{1}]&-k_{t}{\bf I}&k_{t}[{\bf p}_{2}]+d_{t}[\boldsymbol{\omega}_{B}][{\bf p}_{2}]+[{}^{W}{\bf f}_{2}]\\ k_{t}[{\bf p}_{2}]&[{\bf p}_{2}]{\bf J}_{{21}}+k_{r}{}^{W}{\bf U}^{T}&-k_{t}[{\bf p}_{2}]&[{\bf p}_{2}]{\bf J}_{{23}}-k_{r}{}^{W}{\bf U}+[{}^{W}\boldsymbol{\tau}_{2}]\end{matrix}\right),$

(0.59)

again with all quantities expressed in world coordinates and ${}^{W}{\bf U}$ defined by (0.45) with ${\bf u}$ expressed in world coordinates. However, because of rotating coordinate frames, this is not the world coordinate Jacobian. Consider first ${}^{W}{\bf f}_{A}={\bf R}_{{AW}}{}^{A}{\bf f}_{A}$ . Differentiation yields

	$\displaystyle\delta{}^{W}{\bf f}_{A}$	$\displaystyle={\bf R}_{{AW}}\delta{}^{A}{\bf f}_{A}+\delta\boldsymbol{\theta}_{A}\times{}^{W}{\bf f}_{A}$
		$\displaystyle={\bf R}_{{AW}}\delta{}^{A}{\bf f}_{A}-[{}^{W}{\bf f}_{1}]\delta\boldsymbol{\theta}_{A}$

where we have used (0.37) and (0.39) to determine that ${}^{W}{\bf f}_{A}={}^{W}{\bf f}_{1}$ . This eliminates the $[{}^{W}{\bf f}_{1}]$ term in the $(0,1)$ block of (0.59), while a similar derivation eliminates the $[{}^{W}{\bf f}_{2}]$ term in the $(2,3)$ block. Next, from (0.37) and (0.39) we can determine that

${}^{W}\boldsymbol{\tau}_{A}={\bf R}_{{AW}}{}^{A}\boldsymbol{\tau}_{1}+{\bf R}_{{AW}}{\bf p}_{1}\times{}^{W}{\bf f}_{1}$

and therefore

	$\displaystyle\delta{}^{W}\boldsymbol{\tau}_{A}$	$\displaystyle={\bf R}_{{AW}}\delta{}^{A}\boldsymbol{\tau}_{1}+\delta\boldsymbol{\theta}_{A}\times{}^{W}\boldsymbol{\tau}_{1}+(\delta\boldsymbol{\theta}_{A}\times{}^{W}{\bf p}_{1})\times{}^{W}{\bf f}_{1}$
		$\displaystyle={\bf R}_{{AW}}\delta{}^{A}\boldsymbol{\tau}_{1}-[{}^{W}\boldsymbol{\tau}_{1}]\delta\boldsymbol{\theta}_{A}+[{}^{W}{\bf f}_{1}][{}^{W}{\bf p}_{1}]\delta\boldsymbol{\theta}_{A}.$

This eliminates the $[{}^{W}\boldsymbol{\tau}_{1}]$ term from the $(1,1)$ block in (0.59) and adds a $[{}^{W}{\bf f}_{1}][{\bf p}_{1}]$ term in its place. A similar derivation eliminates the $[{}^{W}\boldsymbol{\tau}_{2}]$ term in the $(3,3)$ block and adds $[{}^{W}{\bf f}_{2}][{\bf p}_{2}]$ . Together, these adjustments lead us to the position Jacobian in world coordinates:

${}^{W}{\bf J}_{p}=\left(\begin{matrix}-k_{t}{\bf I}&k_{t}[{\bf p}_{1}]+d_{t}[\boldsymbol{\omega}_{A}][{\bf p}_{1}]&k_{t}{\bf I}&-k_{t}[{\bf p}_{2}]-d_{t}[\boldsymbol{\omega}_{B}][{\bf p}_{2}]\\ -k_{t}[{\bf p}_{1}]&[{\bf p}_{1}]{\bf J}_{{01}}-k_{r}{}^{W}{\bf U}^{T}+[{}^{W}{\bf f}_{1}][{\bf p}_{1}]&k_{t}[{\bf p}_{1}]&[{\bf p}_{1}]{\bf J}_{{03}}+k_{r}{}^{W}{\bf U}\\ k_{t}{\bf I}&-k_{t}[{\bf p}_{1}]-d_{t}[\boldsymbol{\omega}_{A}][{\bf p}_{1}]&-k_{t}{\bf I}&k_{t}[{\bf p}_{2}]+d_{t}[\boldsymbol{\omega}_{B}][{\bf p}_{2}]\\ k_{t}[{\bf p}_{2}]&[{\bf p}_{2}]{\bf J}_{{21}}+k_{r}{}^{W}{\bf U}^{T}&-k_{t}[{\bf p}_{2}]&[{\bf p}_{2}]{\bf J}_{{23}}-k_{r}{}^{W}{\bf U}+[{}^{W}{\bf f}_{2}][{\bf p}_{2}]\end{matrix}\right).$

0.23 Non-symmetry of Jacobians for Rigid Body Forces

Consider a rigid body with a constant force (in world coordinates) ${}^{W}{\bf f}$ acting on its center of mass. The force in body coordinates is given by

${}^{B}{\bf f}={\bf R}_{{WB}}{}^{W}{\bf f}.$

If ${}^{B}\delta\boldsymbol{\omega}\equiv{}^{B}\delta\boldsymbol{\omega}_{{BW}}$ is a variation in the body’s orientation, then the resulting variation ${}^{B}\delta{\bf f}$ is given by

${}^{B}\delta{\bf f}=[{}^{B}\delta\boldsymbol{\omega}_{{WB}}]{\bf R}_{{WB}}{}^{W}{\bf f}=-[{}^{B}\delta\boldsymbol{\omega}_{{BW}}]{\bf R}_{{WB}}{}^{W}{\bf f}=-[{}^{B}\delta\boldsymbol{\omega}]{\bf R}_{{WB}}{}^{W}{\bf f}$

from which we can deduce that

$\frac{\partial{}^{B}{\bf f}}{\partial\boldsymbol{\theta}}=[{\bf R}_{{WB}}{}^{W}{\bf f}]$

which is asymmetrical.

Moreover, this asymmetry cannot be averted by formulating the equations of motion in world coordinates instead of body coordinates, because then the inertia matrix varies with orientation in a way that is also asymmetrical.

To see this, consider an inertia matrix located at the body’s center of mass. In world coordinates, this is

${\bf M}=\left(\begin{matrix}m{\bf I}&0\\ 0&{}^{W}{\bf J}\end{matrix}\right)=\left(\begin{matrix}m{\bf I}&0\\ 0&{\bf R}_{{BW}}{}^{B}{\bf J}{\bf R}_{{WB}}\end{matrix}\right).$

The variation $\delta{\bf M}$ in response to $\delta\boldsymbol{\omega}$ is then

	$\displaystyle\delta{\bf M}$	$\displaystyle=\left(\begin{matrix}0&0\\ 0&\delta{\bf R}_{{BW}}\,{}^{B}{\bf J}{\bf R}_{{WB}}+{\bf R}_{{BW}}{}^{B}{\bf J}\,\delta{\bf R}_{{WB}}\end{matrix}\right)$
		$\displaystyle=\left(\begin{matrix}0&0\\ 0&[{}^{W}\boldsymbol{\omega}]{}^{W}{\bf J}-{}^{W}{\bf J}[{}^{W}\boldsymbol{\omega}]\end{matrix}\right)$

where

${}^{W}\delta\boldsymbol{\omega}\equiv{}^{W}\delta\boldsymbol{\omega}_{{BW}}$

Now consider the implicit integration formula:

	$\displaystyle{\bf v}^{{i+1}}$	$\displaystyle={\bf v}^{i}+h{\bf a}^{{i+1}}$
		$\displaystyle\approx{\bf v}^{i}+h({\bf a}^{i}+\delta{\bf a})$

Since ${\bf a}={\bf M}^{{-1}}{\bf f}$ , we have

$\delta{\bf a}={\bf M}^{{-1}}\delta{\bf f}+\delta({\bf M}^{{-1}}){\bf f}={\bf M}^{{-1}}\delta{\bf f}-{\bf M}^{{-1}}\delta{\bf M}\,{\bf M}^{{-1}}{\bf f}.$

Multiplying the implicit formula through by ${\bf M}$ yeilds

${\bf M}{\bf v}^{{i+1}}={\bf M}{\bf v}^{i}+h({\bf f}^{i}+\delta{\bf f}-\delta{\bf M}\,{\bf M}^{{-1}}{\bf f}).$

Expanding the $\delta{\bf M}\,{\bf M}^{{-1}}{\bf f}$ term, we obtain

	$\displaystyle\delta{\bf M}{\bf M}^{{-1}}{\bf f}$	$\displaystyle=\left(\begin{matrix}0&0\\ 0&[{}^{W}\boldsymbol{\omega}]{}^{W}{\bf J}-{}^{W}{\bf J}[{}^{W}\boldsymbol{\omega}]\end{matrix}\right)\left(\begin{matrix}1/m{\bf I}&0\\ 0&{}^{W}{\bf J}^{{-1}}\end{matrix}\right)\left(\begin{matrix}{\bf f}_{t}\\ \boldsymbol{\tau}\end{matrix}\right)$
		$\displaystyle=\left(\begin{matrix}0\\ ([{}^{W}\boldsymbol{\omega}]{}^{W}{\bf J}-{}^{W}{\bf J}[{}^{W}\boldsymbol{\omega}]){}^{W}{\bf J}^{{-1}}\boldsymbol{\tau}\end{matrix}\right)$

where ${\bf f}_{t}$ and $\boldsymbol{\tau}$ are the translational force and torque. The lower term can be formulated as ${\bf X}\,{}^{W}\boldsymbol{\omega}$ , where

${\bf X}=-[\boldsymbol{\tau}]+{}^{W}{\bf J}[{}^{W}{\bf J}^{{-1}}\boldsymbol{\tau}]$

is clearly assymmetric.

0.1 Spatial Inertia Matrices

0.2 Quaternions

0.3 Equivalence between LCP and Quadratic Programming

0.4 Derivatives of roll, pitch and yaw angles

0.5 Constraint Satisfaction as Projection

0.6 Solution of a general KKT system

0.7 Turning a mixed LCP into a regular LCP

0.8 On demand pivoting for a mixed LCP formulation

0.9 Solvers

0.9.1 Symplectic Euler:

0.9.2 Implicit Euler:

0.9.3 Trapezoidal:

0.9.4 Bridson-Marino:

0.10 Integration with Constraints

0.11 Softening Constraints with Regularization

0.12 Jacobians for Axial Springs

0.12.1 Linear Axial Springs

0.13 Spring Jacobians Formulated using Stress

0.14 Jacobians for Multi-Point Springs

0.15 Gauss Seidel and KKT Systems

0.16 A Gauss Seidel Friction Iteration

0.17 Null-space reduction of the KKT system for attachments

0.18 Attaching to an attached component

0.19 Adjustments to mass and fictitous forces arising from attachments

0.20 Attachments for one-dimensional constraints

0.21 Reduction of KKT system with parametrically controlled variables

0.22 Jacobians for Frame Springs

0.22.1 Position Jacobian

0.22.2 Velocity Jacobian

0.22.3 Jacobians in world coordinates

0.23 Non-symmetry of Jacobians for Rigid Body Forces

Bibliography