\documentclass{jarticle}
\usepackage{amsmath, bm, leftidx, fancybox, ascmac, graphicx}

\title{Featherstone's Algorithm �̒莮��}
\author{tazz}
\date{}

\setlength{\oddsidemargin}{0mm}
\setlength{\textwidth}{160mm}

\begin{document}
\maketitle

\begin{itemize}
\item 05/10/10 ���M�J�n
\item 13/06/01 �ŏI�X�V
\end{itemize}

\section{�ʒu�C���x�C�����x�̓`�d}

�{�e�ł͐Î~���W�n��Î~�t���[���C�^�����W�n��P�Ƀt���[���ƌĂԁD
�Î~�t���[���ɑ΂���t���[���̈ʒu�ƌ�����
\begin{align*}
\begin{bmatrix}\bm{r} \\ R \end{bmatrix}
\end{align*}
�ƕ\������D������$\bm{r}$��3�����x�N�g���C$R$��$3 \times 3$���K�����s��ł���D

�t���[��$a$�ƃt���[��$b$������Ƃ���D���ꂼ��̈ʒu�E������
\begin{align*}
\begin{bmatrix}\bm{r}_a \\ R_a \end{bmatrix}, \;\begin{bmatrix}\bm{r}_b \\ R_b \end{bmatrix}
\end{align*}
�Ƃ��C�t���[��$a$�ɑ΂���t���[��$b$�̑��ΓI�Ȉʒu�E������
\begin{align*}
\begin{bmatrix}\bm{r}_{ab} \\ R_{ab} \end{bmatrix}
\end{align*}
�Ƃ����ƁC�����̊ԂɈȉ��̊֌W�������藧�D
\begin{align}
\begin{bmatrix}\bm{r}_b \\ R_b \end{bmatrix}
 =  \begin{bmatrix}\bm{r}_a \\ R_a \end{bmatrix} \cdot \begin{bmatrix}\bm{r}_{ab} \\ R_{ab} \end{bmatrix}
 =: \begin{bmatrix}\bm{r}_a + R_a \bm{r}_{ab} \\ R_a R_{ab} \end{bmatrix}
\label{position_propagation}
\end{align}

��(\ref{position_propagation})�����Ԕ��������
\begin{align*}
\dot{\bm{r}_b} = \dot{\bm{r}_a} + \dot{R_a}\bm{r}_{ab} + R_a \dot{\bm{r}_{ab}}
 = \dot{\bm{r}_a} + \hat{\bm{\omega}}_a^\times R_a \bm{r}_{ab} + R_a \dot{\bm{r}_{ab}}
\end{align*}
\begin{align*}
&\dot{R_b} = \dot{R_a}R_{ab} + R_a\dot{R_{ab}}\notag\\
&\hat{\bm{\omega_b}}^\times R_b = \hat{\bm{\omega_a}}^\times R_a R_{ab} + R_a \hat{\bm{\omega_{ab}}}^\times R_{ab}\notag\\
&\hat{\bm{\omega_b}}^\times = \hat{\bm{\omega_a}}^\times + R_a \hat{\bm{\omega_{ab}}}^\times R_a^\mathsf{T}
 = \hat{\bm{\omega_a}}^\times + (R_a \hat{\bm{\omega_{ab}}})^\times
\end{align*}
�ƂȂ�D
������$\hat{\bm{\omega}}^\times = \dot{R}R^\mathsf{T}$�͘c�Ώ̍s��ł���C�����x�N�g��$\hat{\bm{\omega}}$�����݂���
$\hat{\bm{\omega}}^\times \bm{a} = \hat{\bm{\omega}} \times \bm{a}$�𖞂����i$\bm{a}$�͔C�Ӂj�D
���̂Ƃ�$\hat{\bm{\omega}}$�͐Î~�t���[���ɑ΂��郍�[�J���t���[���̊p���x��\���D
���߂�$\dot{\bm{r}_a} = \hat{\bm{v}}_a$�C$\dot{\bm{r}_b} = \hat{\bm{v}}_b$�C$\dot{\bm{r}_{ab}} = \bm{v}_{ab}$
�Ƃ����ƁC�����𓾂�D
\begin{align}
\hat{\bm{v}}_b &= \hat{\bm{v}}_a + \hat{\bm{\omega}}_a \times (R_a \bm{r}_{ab}) + R_a \bm{v}_{ab}
\label{vel_prop_abs} \\
\hat{\bm{\omega}}_b &= \hat{\bm{\omega}}_a + R_a \bm{\omega}_{ab}
\label{angvel_prop_abs}
\end{align}
������$\hat{\bm{v}}_a, \hat{\bm{v}}_b$�͂��ꂼ��Î~�t���[���ɑ΂���t���[��a, b�̕��i���x�C
$\hat{\bm{\omega}}_a, \hat{\bm{\omega}}_b$�͓��p���x�ł���D
�܂��C$\bm{v}_{ab}, \bm{\omega}_{ab}$�͂��ꂼ��t���[��a�ɑ΂���t���[��b�̑��Α��x�C���Ίp���x�ł���D

���ɏ㎮�����[�J�����W�\���ɕό`����D�܂��C�t���[���̑��x�E�p���x���t���[�����g�̍��W�ɂ���ĕ\���������̂�
�ȉ��̂悤�ɒ�`����D
\begin{align*}
\bm{v}_a = R_a^\mathsf{T} \hat{\bm{v}}_a, \bm{\omega}_a = R_a^\mathsf{T} \hat{\bm{\omega}}_a,\,
\bm{v}_b = R_b^\mathsf{T} \hat{\bm{v}}_b, \bm{\omega}_b = R_b^\mathsf{T} \hat{\bm{\omega}}_b
\end{align*}
����ƁC���[�J�����W�\���ɂ�鑬�x�`�d��
\begin{align*}
\bm{v}_b &= R_{ab}^\mathsf{T} \bm{v}_a + R_{ab}^\mathsf{T}(\bm{\omega}_a \times \bm{r}_{ab}) + R_{ab}^\mathsf{T} \bm{v}_{ab}\\
\bm{\omega}_b &= R_{ab}^\mathsf{T} \bm{\omega}_a + R_{ab}^\mathsf{T} \bm{\omega}_{ab}
\end{align*}
�ƂȂ�D
������
\begin{align*}
X(R, \bm{r}) = \begin{bmatrix}R^\mathsf{T} & -R^\mathsf{T} \bm{r}^\times \\ O & R^\mathsf{T}\end{bmatrix}
\end{align*}
�ƒ�`���C�����
\begin{align*}
\bm{V}_a = \begin{bmatrix}\bm{v}_a \\ \bm{\omega}_a\end{bmatrix},\,
\bm{V}_b = \begin{bmatrix}\bm{v}_b \\ \bm{\omega}_b\end{bmatrix},\,
\bm{V}_{ab} = \begin{bmatrix}\bm{v}_{ab} \\ \bm{\omega}_{ab}\end{bmatrix}
\end{align*}
�Ƃ����ƁC
\begin{align}
\bm{V}_b = X(R_{ab}, \bm{r}_{ab}) \bm{V}_a + X(R_{ab}, \bm{0}) \bm{V}_{ab}
\label{vel_prop}
\end{align}
�ƊȌ��ɏ�����D
%�t���[��a, b�Ԃ̑��ΓI�Ȉʒu�E���������ԓI�ɕω����Ȃ��Ȃ�ΒP��$\bm{V}_b = X_{ba} \bm{V}_a$�ƂȂ�D

�ϊ�$X(R, \bm{r})$�͈ȉ��̊�{�I�Ȑ����𖞂����D
\begin{align}
\begin{aligned}
X(R_a, \bm{r}_a) X(R_b, \bm{r}_b)
&= \begin{bmatrix}R_a^\mathsf{T} & -R_a^\mathsf{T} \bm{r}_a^\times \\ O & R_a^\mathsf{T}\end{bmatrix}
   \begin{bmatrix}R_b^\mathsf{T} & -R_b^\mathsf{T} \bm{r}_b^\times \\ O & R_b^\mathsf{T}\end{bmatrix} \\
&= \begin{bmatrix}(R_b R_a)^\mathsf{T} & -(R_b R_a)^\mathsf{T} (\bm{r}_b + R_b \bm{r}_a)^\times \\ O & (R_b R_a)^\mathsf{T}\end{bmatrix} \\
&= X(R_b R_a, \bm{r}_b + R_b \bm{r}_a)
\end{aligned}
\end{align}
\begin{align}
X(R, \bm{r})^{-1} &= X(R^\mathsf{T}, -R^\mathsf{T} \bm{r})
\end{align}

�����ĉ����x�`�d�̎��𓱏o����D
��(\ref{vel_prop_abs})(\ref{angvel_prop_abs})���X�Ɏ��Ԕ��������
\begin{align*}
\dot{\hat{\bm{v}}}_b
	&= \dot{\hat{\bm{v}}}_a + \dot{\hat{\bm{\omega}}}_a \times (R_a \bm{r}_{ab}) +
	\hat{\bm{\omega}}_a \times (\hat{\bm{\omega}}_a \times (R_a \bm{r}_{ab}) + R_a \dot{\bm{r}}_{ab}) +
	\hat{\bm{\omega}}_a \times (R_a \bm{v}_{ab}) + R_a \dot{\bm{v}}_{ab} \notag\\
	&= \dot{\hat{\bm{v}}}_a + \dot{\hat{\bm{\omega}}}_a \times (R_a \bm{r}_{ab}) +
	(\hat{\bm{\omega}}_a \cdot (R_a \bm{r}_{ab}))\hat{\bm{\omega}}_a - ||\hat{\bm{\omega}}_a||^2(R_a \bm{r}_{ab}) +
	2 \, \hat{\bm{\omega}}_a \times (R_a \bm{v}_{ab}) + R_a \dot{\bm{v}}_{ab} \notag\\
	&= \dot{\hat{\bm{v}}}_a + \dot{\hat{\bm{\omega}}}_a \times (R_a \bm{r}_{ab}) +
	(\bm{\omega}_a \cdot \bm{r}_{ab})\hat{\bm{\omega}}_a - ||\bm{\omega}_a||^2(R_a \bm{r}_{ab}) +
	2 \, \hat{\bm{\omega}}_a \times (R_a \bm{v}_{ab}) + R_a \dot{\bm{v}}_{ab} \\
\dot{\hat{\bm{\omega}}}_b
	&= \dot{\hat{\bm{\omega}}}_a + \hat{\bm{\omega}}_a \times (R_a \bm{\omega}_{ab}) + R_a \dot{\bm{\omega}}_{ab}
\end{align*}
�ƂȂ�D
�������x�N�g���O�d��$\bm{a} \times (\bm{b} \times \bm{c}) = (\bm{a} \cdot \bm{c})\bm{b} - (\bm{a} \cdot \bm{b})\bm{c}$�ɒ��ӁD
�V����$\hat{\bm{a}}_a := \dot{\hat{\bm{v}}}_a,\, \hat{\bm{\alpha}}_a := \dot{\hat{\bm{\omega}}}_a$, 
$\hat{\bm{a}}_b := \dot{\hat{\bm{v}}}_b,\, \hat{\bm{\alpha}}_b := \dot{\hat{\bm{\omega}}}_b$,
�����
$\bm{a}_{ab} := \dot{\bm{v}}_{ab},\, \bm{\alpha}_{ab} := \dot{\bm{\omega}}_{ab}$
�ƒ�`����ƁC
\begin{align}
\hat{\bm{a}}_b &= \hat{\bm{a}}_a + \hat{\bm{\alpha}}_a \times (R_a \bm{r}_{ab}) +
	(\bm{\omega}_a \cdot \bm{r}_{ab})\hat{\bm{\omega}}_a - ||\bm{\omega}_a||^2(R_a \bm{r}_{ab}) +
	2 \, \hat{\bm{\omega}}_a \times (R_a \bm{v}_{ab}) + R_a \bm{a}_{ab}
\label{accel_prop_abs}\\
\hat{\bm{\alpha}}_b
	&= \hat{\bm{\alpha}}_a + \hat{\bm{\omega}}_a \times (R_a \bm{\omega}_{ab}) + R_a \bm{\alpha}_{ab}
\label{angaccel_prop_abs}
\end{align}
�ƂȂ�D
���x�Ɠ��l�Ƀ��[�J�����W�\���ɕϊ�����D
�܂��ȉ��̂悤�ɒ�`����D
\begin{align*}
\bm{a}_a = R_a^\mathsf{T} \hat{\bm{a}}_a, \, \bm{\alpha}_a = R_a^\mathsf{T} \hat{\bm{\alpha}}_a, \,
\bm{a}_b = R_b^\mathsf{T} \hat{\bm{a}}_b, \, \bm{\alpha}_b = R_b^\mathsf{T} \hat{\bm{\alpha}}_b
\end{align*}
\begin{align*}
\bm{A}_a = \begin{bmatrix}\bm{a}_a \\ \bm{\alpha}_a\end{bmatrix},\,
\bm{A}_b = \begin{bmatrix}\bm{a}_b \\ \bm{\alpha}_b\end{bmatrix},\,
\bm{A}_{ab} = \begin{bmatrix}\bm{a}_{ab} \\ \bm{\alpha}_{ab}\end{bmatrix}
\end{align*}
��(\ref{accel_prop_abs})(\ref{angaccel_prop_abs})�̗��ӂ�$R_b^\mathsf{T}$�������Đ�������ƁC�����𓾂�D
\begin{align}
\bm{A}_b = X(R_{ab}, \bm{r}_{ab}) \bm{A}_a + X(R_{ab}, \bm{0}) (\bm{C}_{ab} + \bm{A}_{ab})
\label{accel_prop}
\end{align}
������$\bm{C}_{ab}$��
\begin{align*}
\bm{C}_{ab} = \begin{bmatrix}
	(\bm{\omega}_a \cdot \bm{r}_{ab})\bm{\omega}_a - ||\bm{\omega}_a||^2 \bm{r}_{ab} + 2\,\bm{\omega}_a \times \bm{v}_{ab}\\
	\bm{\omega}_a \times \bm{\omega}_{ab}
\end{bmatrix}
\end{align*}
�ƕ\����C�R���I���̉����x�Ɖ��S�����x���琬��D

\section{�֐ߍ��̌n�̃��f��}
�֐߂ɂ���ĂȂ��ꂽ��̍��̂̋@�\�w�I�֌W���𓱏o����D

\begin{figure}
\label{notation}
\end{figure}

Fig.\,\ref{notation}�͊֐߂ɂ���ĂȂ��ꂽ��̍��̂̈ʒu�֌W��\�킵�Ă���D
$p, c, j, j'$�̊e�Y�����͐e���́C�q���́C�\�P�b�g�i�e���̂Ɏ��t����ꂽ�֐߁j�C�v���O�i�q���̂Ɏ��t����ꂽ�֐߁j�������D

\subsection{���x�`�d}

�e�t���[���Ԃ̑��x�̊֌W�͎�(\ref{vel_prop})���
\begin{align*}
&\bm{V}_j = X(R_{pj}, \bm{r}_{pj}) \bm{V}_p \\
&\bm{V}_{j'} = X(R_{jj'}, \bm{r}_{jj'}) \bm{V}_j + X(R_{jj'}, \bm{0}) \bm{V}_{jj'}\\
&\bm{V}_{j'} = X(R_{cj'}, \bm{r}_{cj'}) \bm{V}_c
\end{align*}
�ƂȂ�D�㎮����$\bm{V}_j$, $\bm{V}_{j'}$����������ƁC$\bm{V}_p$����$\bm{V}_c$�ւ̑��x�`�d��
\begin{align}
\bm{V}_c &= X(R_{cj'}, \bm{r}_{cj'})^{-1} X(R_{jj'}, \bm{r}_{jj'}) X(R_{pj}, \bm{r}_{pj}) \bm{V}_p
		 + X(R_{cj'}, \bm{r}_{cj'})^{-1} X(R_{jj'}, \bm{0}) \bm{V}_{jj'}
\label{vel_prop2}
\end{align}
�𓾂�D
�֐ߑ��x$\bm{V}_{jj'}$�͊֐ߕϐ�$\bm{q}_c$����ъ֐ߑ��x$\dot{\bm{q}}_c$��p����
\begin{align}
\bm{V}_{jj'} = J_c(\bm{q}_c) \dot{\bm{q}}_c
\label{joint_vel}
\end{align}
�ƕ\�킹��D$J_c$�͊֐߂̎�ނɂ���Č��܂郄�R�r�s��ł���D
�X�̊֐߂̎�ނɑΉ����郄�R�r�s���\ref{jacobian}�߂��Q�ƁD
��(\ref{joint_vel})����(\ref{vel_prop2})�ɑ�����邱�Ƃɂ��
\begin{align}
\bm{V}_c = X_{cp} \bm{V}_p + \hat{J}_c \dot{\bm{q}}_c
\label{vel_prop3}
\end{align}
�ƂȂ�D
������
\begin{align*}
X_{cp} &= X(R_{cj'}, \bm{r}_{cj'})^{-1} X(R_{jj'}, \bm{r}_{jj'}) X(R_{pj}, \bm{r}_{pj})\\
X_{cj} &= X(R_{cj'}, \bm{r}_{cj'})^{-1} X(R_{jj'}, \bm{0})\\
\hat{J}_c &= X_{cj} J_c
\end{align*}
�ł���D

\subsection{�����x�`�d}

�e�t���[���Ԃ̉����x�̊֌W�͎�(\ref{accel_prop})���
\begin{align*}
&\bm{A}_j = X(R_{pj}, \bm{r}_{pj}) \bm{A}_p + X(R_{pj}, \bm{0}) C_{pj}\\
&\bm{A}_{j'} = X(R_{jj'}, \bm{r}_{jj'}) \bm{A}_j + X(R_{jj'}, \bm{0}) (C_{jj'} + \bm{A}_{jj'})\\
&\bm{A}_{j'} = X(R_{cj'}, \bm{r}_{cj'}) \bm{A}_c + X(R_{cj'}, \bm{0}) C_{cj'}
\end{align*}
�ƂȂ�D�㎮����$\bm{A}_j$, $\bm{A}_{j'}$����������ƁC$\bm{A}_p$����$\bm{A}_c$�ւ̉����x�`�d��
\begin{align}
\bm{A}_c &= X(R_{cj'}, \bm{r}_{cj'})^{-1} X(R_{jj'}, \bm{r}_{jj'}) X(R_{pj}, \bm{r}_{pj}) \bm{A}_p \notag\\
	&+ X(R_{cj'}, \bm{r}_{cj'})^{-1} X(R_{jj'}, \bm{0}) \bm{A}_{jj'} \notag\\
	&+ X(R_{cj'}, \bm{r}_{cj'})^{-1}(
		X(R_{jj'}, \bm{r}_{jj'}) X(R_{pj}, \bm{0}) C_{pj} + X(R_{jj'}, \bm{0}) C_{jj'} - X(R_{cj'}, \bm{0}) C_{cj'})
\label{acc_prop2}
\end{align}
�𓾂�D
����C��(\ref{joint_vel})�����Ԕ������邱�Ƃɂ��
\begin{align}
\bm{A}_{jj'} = J_c(\bm{q}_c) \ddot{\bm{q}}_c + \dot{J}_c(\bm{q}_c, \dot{\bm{q}}_c) \dot{\bm{q}}_c
\label{joint_acc}
\end{align}
�ƂȂ�D
��(\ref{joint_acc})����(\ref{acc_prop2})�ɑ�������
\begin{align}
\bm{A}_c = X_{cp} \bm{A}_p + \hat{C}_{pc} + \hat{J}_c \ddot{\bm{q}}_c
\label{acc_prop3}
\end{align}
�𓾂�D������
\begin{align}
\hat{C}_{pc} &= X(R_{cj'}, \bm{r}_{cj'})^{-1}\left(
		X(R_{jj'}, \bm{r}_{jj'}) X(R_{pj}, \bm{0}) C_{pj} + X(R_{jj'}, \bm{0}) C_{jj'} - X(R_{cj'}, \bm{0}) C_{cj'}\right)
	+ X_{cj} \dot{J}_c \dot{\bm{q}}_c
\end{align}
�ł���D

\subsection{Articulated Body Algorithm�̓��o}

Articulated Body Algorithm (ABA) �́C�؍\���𐬂��A�����̌n�̊�{�I�Ȑ����ɂ��ƂÂ��Ă���D
�؍\���Ɋւ��C����$i$�̎q���̂̏W����$\mathcal{C}_i$�ƕ\�L����D

\begin{itembox}[l]{\textbf{[�藝1]}}
�؍\���𐬂��A�����̌n�̊e����$i$�ɂ��āC���������藧�D
\begin{align}
\bm{F}_i = \hat{I}_i \bm{A}_i + \hat{Z}_i
\label{eq_aba}
\end{align}
������$\bm{F}_i$�͍���$i$�Ƃ��̐e���̂��Ȃ��֐߂����p����́C$\bm{A}_i$�͍���$i$�̉����x�ł���D
�܂��C�s��$\hat{I}_i$�ƃx�N�g��$\hat{Z}_i$��$\{\hat{I}_c�C\, \hat{Z}_c\}$ ($c \in \mathcal{C}_i$)�ɂ�茈�肳���D
\end{itembox}
$\hat{I}_i$�͍���$i$�����Ƃ��镔���؂̊�����\���s��ł���Carticulated inertia matrix���Ă΂��D
�܂��C$\hat{Z}_i$�͉����x$\bm{A}_i$��$0$�̂Ƃ���$\bm{F}_i$�Ƃ肠���͂ł��邱�Ƃ���articulated bias force�ƌĂ΂��D

\flushleft{�ؖ�)}\\
\flushleft{[�I�[�m�[�h�̃P�[�X]}\\
����$i$�ɂ���
\begin{align}
I_i &= \begin{bmatrix}m_i \bm{1} & O \\ O & J_i \end{bmatrix}, \label{eq_I_initial} \\
Z_i &= - (\bm{F}_i + \bm{F}^\mathrm{ext}_i)                    \label{eq_Z_initial}
\end{align}
�Ƃ����D
������$m_i$�͍���$i$�̎��ʁC$J_i$�͏d�S�Ɋւ��銵���e���\���C$\bm{1}$��$3\times3$�̒P�ʍs��ł���D
�܂��C$\bm{F}_i = \begin{bmatrix}\bm{0} \\ -\bm{\omega}_i \times J_i \bm{\omega}_i\end{bmatrix}$�̓R���I���́C
$\bm{F}^\mathrm{ext}_i$�͂��̑��̊O�͂ł���D
����ƃc���[�̖��[�ɂ��鍄��$i$�̉^���������͎��̂悤�ɏ�����D
\begin{align*}
I_i \bm{A}_i = \bm{F}^\mathrm{I}_i - Z_i
\end{align*}
������$\bm{F}^\mathrm{I}_i$�͐e�m�[�h����֐߂���Ď󂯂�͂ŁC�{���̍�p�_���獄��$i$�̃t���[������ɓ����ϊ��������̂ł���D
���̂Ƃ�
\begin{align*}
\hat{I}_i &= I_i\\
\hat{Z}_i &= Z_i
\end{align*}
�Ƃ����Ǝ�(\ref{eq_aba})�����藧�D

\flushleft{[���ԃm�[�h�̃P�[�X]}\\
�c���[�̒��ԂɈʒu���鍄��$p$���l����D
���ׂĂ̎q����$c \in \mathcal{C}_p$�ɂ���$\bm{F}_c = \hat{I}_c \bm{A}_c + \hat{Z}_c$�����藧�Ƃ���D
����Ɏ�(\ref{acc_prop3})��������ƁC
\begin{align}
\bm{F}_c = \hat{I}_c (X_{cp} \bm{A}_p + \hat{C}_{pc} + \hat{J}_c \ddot{\bm{q}}_c) + \hat{Z}_c
\label{child}
\end{align}
�𓾂�D
����C�֐߃g���N��$\bm{\tau}_c$�������Ɖ��z�d���̌������ȉ������藧�D
\begin{align}
\hat{J}_c^\mathsf{T} \bm{F}_c = \bm{\tau}_c
\label{joint_torque}
\end{align}
��(\ref{child})(\ref{joint_torque})��A������$\ddot{\bm{q}}$�ɂ��ĉ����ƁC
\begin{align}
\ddot{\bm{q}}_c = (\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1}
	(\bm{\tau}_c - (\hat{I}_c \hat{J}_c)^\mathsf{T} X_{cp} \bm{A}_p - \hat{J}_c^\mathsf{T} (\hat{I}_c \hat{C}_{pc} + \hat{Z}_c))
\label{joint_acc2}
\end{align}
����C�e�m�[�h�ɂ�����^���������͈ȉ��̂悤�ɏ�����D
\begin{align}
\bm{F}_p &= I_p \bm{A}_p + Z_p + \sum_{c = 0}^{C_p} X_{cp}^\mathsf{T} \bm{F}_c
\label{parent1}
\end{align}
�E�ӂ̑�3���́C�e�m�[�h���e�q�m�[�h�ɉ�����S���͂̔���p�͂ł���D
����Ɏ�(\ref{child})(\ref{joint_acc2})�������邱�Ƃɂ��C�����𓾂�D
\begin{align}
\bm{F}_p &= \hat{I}_p \bm{A}_p + \hat{Z}_p\\
\hat{I}_p &= I_p + \sum_{c=0}^{C_p} X_{cp}^\mathsf{T} \left\{
	\hat{I}_c  - (\hat{I}_c \hat{J}_c)(\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1} (\hat{I}_c \hat{J}_c)^\mathsf{T} \right\} X_{cp}
\label{eq_I_recurs} \\
\hat{Z}_p &= Z_p + \sum_{c=0}^{C_p} X_{cp}^\mathsf{T} \left\{
	\hat{I}_c \hat{C}_{pc} + \hat{Z}_c
	 + \hat{I}_c \hat{J}_c (\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1} (\bm{\tau}_c - \hat{J}_c^\mathsf{T} (\hat{I}_c \hat{C}_{pc} + \hat{Z}_c))\right\}
\label{eq_Z_recurs}
\end{align}

�ȏ���C�c���[��̔C�ӂ̍��̂ɂ��Ď�(\ref{eq_aba})���������邱�Ƃ��ؖ����ꂽ�D

\subsection{ABA�̃A���S���Y��}

\begin{enumerate}
\item �؍\���̖��[���獪�Ɍ������C��(\ref{eq_I_recurs})(\ref{eq_Z_recurs})�ɂ��$\hat{I}_i$�C$\hat{Z}_i$���v�Z����
\item �����̂ɂ��Ď�(\ref{eq_aba})��������x$\bm{A}_0$�����߂�
\item �؍\���̍����疖�[�Ɍ������C�܂���(\ref{joint_acc2})���֐߉����x$\ddot{q}_i$�����߁C���Ɏ�(\ref{acc_prop3})���
���̂̉����x$\bm{A}_i$�����߂�
\item �e���̂ɑ΂��āC�����x��ϕ����đ��x�C�ʒu���X�V����
\end{enumerate}

\subsection{�֐߂̋L�q}
\subsubsection{1����]�֐�}

�\�P�b�g��Z������]���Ƃ���D
\begin{align*}
	&q \in \Re^1\\
	&R_{jj'} = R_\mathrm{z}(q) =: \begin{bmatrix}\cos(q) & -\sin(q) & 0\\ \sin(q) & \cos(q) & 0\\ 0 & 0 & 1\end{bmatrix}\\
	&\bm{r}_{jj'} = \bm{0}\\
	&J = \begin{bmatrix}0 & 0 & 0 & 0 & 0 & 1\end{bmatrix}^\mathsf{T}
\end{align*}

\subsubsection{1�������֐�}

�\�P�b�g��Z���𒼓����Ƃ���ꍇ�D
\begin{align*}
	&q \in \Re^1\\
	&R_{jj'} = \bm{1}\\
	&\bm{r}_{jj'} = \begin{bmatrix}0 & 0 & q\end{bmatrix}^\mathsf{T}\\
	&J = \begin{bmatrix}0 & 0 & 1 & 0 & 0 & 0\end{bmatrix}^\mathsf{T}
\end{align*}
	
\subsubsection{�{�[���W���C���g}

�֐ߍ��W��$x, y, z$�e���Ɋւ����]�p�x�ŕ\������D
\begin{align*}
	& \bm{q} = [q_x \; q_y \; q_z]^\mathsf{T}\\
	& R_{jj'} = R_\mathrm{z}(q_z)\,R_\mathrm{y}(q_y)\,R_\mathrm{x}(q_x)\\
	& \bm{r}_{jj'} = \bm{0}\\
	& J = \begin{bmatrix}
		0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0\\ \cos(q_y)\cos(q_z) & -\sin(q_z) & 0\\ \cos(q_y)\sin(q_z) & \cos(q_z) & 0\\ -\sin(q_y) & 0 & 1
		\end{bmatrix}
\end{align*}
���͈͐����̓X�C���O�p�ƃc�C�X�g�p�Ɋւ��ĉ���������g���₷���D
�����Ń\�P�b�g�ƃv���O��$z$���������p���X�C���O�p�Ƃ���D
\begin{align}
	\theta_\mathrm{swing} := \cos^{-1}(R_{zz})
\end{align}
����C���҂�$z$���ɒ������鎲�Ɋւ���$z$�����m����v����悤�ɉ�]���������$x$�����m�i$y$�����m�ł������j�������p���c�C�X�g�p�ƒ�`����D
�܂�$z$���m����v�������]�s���
\begin{align}
	\begin{bmatrix}
		R_{zy}^2 / (1 + R_{zz}) + R_{zz} & - R_{zy}R_{zx} / (1 + R_{zz}) & -R_{zx}\\
		- R_{zy}R_{zx} / (1 + R_{zz}) & R_{zx}^2 / (1 + R_{zz}) + R_{zz} & -R_{zy}\\
		R_{zx} & R_{zy} & R_{zz}
	\end{bmatrix}
\end{align}
�Ɨ^������i�m���߂�j�D���������ăc�C�X�g�p��
\begin{align}
	\theta_{twist} := \cos^{-1}((R_{zy}^2R_{xx} + (1 + R_{zz}) R_{zz}R_{xx} - R_{zy}R_{zx}R_{xy} - (1 + R_{zz}R_{zx}R_{xz})/(1+R_{zz}))
\end{align}
���Ȃ킿�C�X�C���O�p��$\bm{q}_{jj'}$�̎��x�N�g���ƃ\�P�b�g��z���������p�C�c�C�X�g�p��$\bm{q}_{jj'}$�̉�]�p�ł���D	

\subsection{����A��}

�M�A��2�̊֐߂̊֐ߍ��W�ɐ��`�ȍS����������v�f�ł���D
�M�A��ABA�ɓK�؂ɑg�ݓ���邱�Ƃɂ��C�l�����ׂ����R�x���팸���v�Z�R�X�g��ጸ�ł���D
������ABA�ň�����̂́C�M�A�����ڂ̐e�q�֌W�ɂ���֐ߓ��m�ɍ�p����ꍇ�i����A���j�ƁC
���ʂ̐e�����֐ߓ��m�ɍ�p����ꍇ�i����A���j�݂̂ł���D
����ʓI�ȏꍇ��LCP�ň����D

���񃊃��N�łȂ�����3�̍���(���ꂼ�ꍄ��0, 1, 2�j������C
����0��1�C����1��2��A������֐߂��M�A�ŘA������ꍇ���l����D
�M�A���$\gamma$�Ƃ���Ɗ֐ߍ��W�ɉ����S����$\dot{\bm{q}}_2 = \gamma \dot{\bm{q}}_1$�Ə�����D
����ƁC���x�`�d�Ɖ����x�`�d�͂��ꂼ�ꎟ�̂悤�ɏ����������D
\begin{align}
	\bm{V}_2 &= X_{21} \bm{V}_1 + \hat{J}_2 \dot{\bm{q}}_2 \\
		&= X_{21} (X_{10} \bm{V}_0 + \hat{J}_1 \dot{\bm{q}}_1) + \hat{J}_2 \dot{\bm{q}}_2 \\
		&= (X_{21} X_{10}) \bm{V}_0 + (X_{21} \hat{J}_1 + \gamma \hat{J}_2) \dot{\bm{q}}_1
\end{align}
\begin{align}
	\bm{A}_2 &= X_{21} \bm{A}_1 + \hat{C}_{12} + \hat{J}_2 \ddot{\bm{q}}_2 \\
		&= X_{21} (X_{10} \bm{A}_0 + \hat{C}_{01} + \hat{J}_1 \ddot{\bm{q}}_1) + \hat{C}_{12} + \hat{J}_2 \ddot{\bm{q}}_2 \\
		&= (X_{21} X_{10})\bm{A}_0 + (X_{21}\hat{C}_{01} + \hat{C}_{12}) + (X_{21}\hat{J}_1 + \gamma \hat{J}_2)\ddot{\bm{q}}_1
\end{align}
���̂悤�ɁC�M�A�ɂ��A���͍���2������0�ɒ��ژA������Ă���ꍇ�Ɠ��l�Ɉ����邱�Ƃ�������D
�ȍ~�͎��߂ŏq�ׂ�A���֐߂ɋA�������D

\subsection{����A��}
�O�߂܂ł�1�̊֐߂�1�g�̍��̂�A������ꍇ���l���Ă������C
1�̊֐߂ɂ�蕡���̍��̂�����ɘA�������ꍇ��������Ƃ��ꂵ�����Ƃ����܂ɂ���D
���ɑO�߂ŏq�ׂ�����A���i�M�A�Ȃǂ̕\���j�͕���A���ɋA�������D

�e����$p$�Ɏq����$c = 1, 2, \dots, C_p$���A������Ă���Ƃ���D
����A���ɂ������x�`�d�͎����ŕ\�������D
\begin{align}
	\bm{A}_c = X_{cp} \bm{A}_p + \hat{C}_{pc} + \hat{J}_c \ddot{\bm{q}} \;\;\;\; (c = 1, 2, \dots, C_p)
\end{align}
������$\bm{q}$�͋��ʂ̊֐ߍ��W�ł���D
ABA�A���S���Y���̏C���_���q�ׂ�D�܂��S�Ă̎q���̂ɂ���$\bm{F}_c = \hat{I}_c \bm{A}_c + \hat{Z}_c$
�����藧�Ƃ���D
���z�d���̌������
\begin{align}
	\bm{\tau} = \sum_{c = 1}^{C_p} \hat{J}_c^\mathsf{T} \bm{F}_c
	\label{joint_torque_parallel}
\end{align}
�����藧�D����������
\begin{align}
	\ddot{\bm{q}} = \left(\sum_{c = 1}^{C_p} \hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c\right)^{-1}
		\left(\bm{\tau} - \left(\sum_{c = 1}^{C_p} X_{cp}^\mathsf{T} \hat{I}_c \hat{J}_c\right)^\mathsf{T} \bm{A}_p - \sum_{c = 1}^{C_p}\hat{J}_c^\mathsf{T} (\hat{I}_c \hat{C}_{pc} + \hat{Z}_c)\right)
	\label{joint_acc2_parallel}
\end{align}
\begin{align}
	\hat{I}_p &= I_p + \sum_{c=1}^{C_p} X_{cp}^\mathsf{T} \hat{I}_c X_{cp} 
		 - \left(\sum_{c=1}^{C_p} X_{cp}^\mathsf{T} \hat{I}_c \hat{J}_c\right)
		   \left(\sum_{c=1}^{C_p} \hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c\right)^{-1}
		   \left(\sum_{c=1}^{C_p} X_{cp}^\mathsf{T} \hat{I}_c \hat{J}_c\right)^\mathsf{T} \\
	\hat{Z}_p &= Z_p + \sum_{c=1}^{C_p} X_{cp}^\mathsf{T} (\hat{I}_c \hat{C}_{pc} + \hat{Z}_c)
		+ \left(\sum_{c=1}^{C_p} X_{cp}^\mathsf{T} \hat{I}_c \hat{J}_c\right)
		  \left(\sum_{c=1}^{C_p} \hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c\right)^{-1}
		  \left(\sum_{c=1}^{C_p} (\bm{\tau}_c - \hat{J}_c^\mathsf{T} (\hat{I}_c \hat{C}_{pc} + \hat{Z}_c))\right)
\end{align}
�����ł͑S�Ă̎q���̂����ʂ�$\bm{q}$�ŘA������Ƃ������C����ʓI�ȏꍇ�ւ̊g���͗e�Ղł���D

\subsection{�c���[�ɑ����鍄�̊Ԃ̊���}

\begin{figure}[t]
\begin{center}
\begin{tabular}{ccc}
\includegraphics[width=40mm]{fig/case_a.eps} &
\includegraphics[width=40mm]{fig/case_b.eps} &
\includegraphics[width=40mm]{fig/case_c.eps} \\
(a) &
(b) &
(c)
\end{tabular}
\end{center}
\end{figure}

ABA��LCP�Ȃǂɂ�锽���^�S���͌v�Z�@�Ƒg�ݍ��킹��ꍇ�C
�c���[�ɉ����S���͂̔����ω��ɑ΂��ăc���[�ɑ����鍄�̂̉����x�̔����ω����J��Ԃ��v�Z���邱�ƂɂȂ�D

��{�I�Ȍv�Z�̗���͈ȉ��̂悤�ɂȂ�D
�S���͂�����鍄�̂�����$i$�Ƃ��C�S���͂̕ω��ʂ�$\delta f$�Ƃ���D
�c���[�ɑ΂��čS���͂͊O�͂ł���̂ŁC��(\ref{eq_Z_initial})���$Z_i$�̕ω��ʂ�
\begin{align*}
\delta Z_i = - \delta f
\end{align*}
�ƂȂ�C�����Ɏ�(\ref{eq_Z_recurs})���
\begin{align*}
\delta \hat{Z}_i = \delta Z_i
\end{align*}
�ƂȂ�D
����$\hat{Z}_i$�̕ω����e���̂ɓ`�d����D
����͎�(\ref{eq_Z_recurs})���
\begin{align*}
\delta \hat{Z}_p &= X_{cp}^\mathsf{T} 
 \left\{
  \bm{1} - \hat{I}_c \hat{J}_c (\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1} \hat{J}_c^\mathsf{T}
 \right\} \delta \hat{Z}_c
\end{align*}
�ƂȂ�D
���̊֌W���ɂ�荪���̂�$\delta \hat{Z}_0$�܂ŋ��߂�D
����ɑ΂��鍪���̂̉����x�̕ω��ʂ͎�(\ref{eq_aba})���
\begin{align*}
\delta \bm{A}_0 = - \hat{I}_0^{-1} \delta \hat{Z}_0
\end{align*}
�ƂȂ�D
���x�͉����x�`�d���ɂ��ƂÂ������̉����x�ω��ʂ����߂Ă����D
�e���̂Ǝq���̂̉����x�ω��̓`�d���͈ȉ��̂悤�ɂȂ�D
�܂���(\ref{joint_acc2})���
\begin{align*}
\delta \ddot{\bm{q}}_c = -(\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1}
	((\hat{I}_c \hat{J}_c)^\mathsf{T} X_{cp} \delta \bm{A}_p + \hat{J}_c^\mathsf{T} \delta \hat{Z}_c)
\end{align*}
�ƂȂ�C���Ɏ�(\ref{acc_prop3})���
\begin{align*}
\delta \bm{A}_c = X_{cp} \delta \bm{A}_p + \hat{J}_c \delta \ddot{\bm{q}}_c
\end{align*}
�ƂȂ�D
$\ddot{q}_c$�����������
\begin{align*}
\delta \bm{A}_c = (\bm{1} - \hat{J}_c (\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1}(\hat{I}_c \hat{J}_c)^\mathsf{T}) X_{cp} \delta \bm{A}_p
 - \hat{J}_c (\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1} \hat{J}_c^\mathsf{T} \delta \hat{Z}_c
\end{align*}
�ƂȂ�D
���āC������
\begin{align*}
T_{cp} &= (\bm{1} - \hat{J}_c (\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1}(\hat{I}_c \hat{J}_c)^\mathsf{T}) X_{cp}, \\
S_{cp} &= \hat{J}_c (\hat{J}_c^\mathsf{T} \hat{I}_c \hat{J}_c)^{-1} \hat{J}_c^\mathsf{T} \delta \hat{Z}_c
\end{align*}
�Ƃ����Ώ㎮�͊ȒP��
\begin{align*}
\delta \hat{Z}_p &= T_{cp}^\mathsf{T} \, \delta \hat{Z}_c, \\
\delta \bm{A}_c  &= T_{cp} \, \delta \bm{A}_p - S_{cp} \, \delta \hat{Z}_c
\end{align*}
�Ə�����D
�����Œ��ӂ�v����̂́C�ŏ��ɍS���͕ω�������������$i$�ƍ����̂����Ԍo�H��̍��̂͗�łȂ�$\delta \hat{Z}$�����̂ɑ΂��C
����ȊO�̍��̂ɂ��Ă�$\delta \hat{Z} = 0$�ł���_�ł���D
���̂��߁C��ʂɍ���$i$�ł̍S���͕ω��ɂ���Đ����鍄��$j$�ł̉����x�ω������߂�ɂ́C
����$i$��$j$�̋��ʂ̐e����$p$���l���C
\begin{enumerate}
\item ����$i$���獄��$p$�ւ�$\delta \hat{Z}$�`�d
\item ����$p$���獪���̂ւ�$\delta \hat{Z}$�`�d
\item �����̂��獄��$p$�ւ�$\delta \bm{A}$�`�d
\item ����$p$���獄��$j$�ւ�$\delta \bm{A}$�`�d
\end{enumerate}
�ɕ������čl����K�v������D
����ȏꍇ�Ƃ��č���$i$������$j$�̏㗬�ɂ���ꍇ�i���̂Ƃ�����$p$�͍���$i$�ł���j�ƁC
�t�ɍ���$i$������$j$�̉����ɂ���ꍇ�i���̂Ƃ�����$p$�͍���$j$�ł���j���܂܂��D

�����ŃX�e�b�v2��3�͂܂Ƃ߂č���$p$��$\delta \hat{Z}_p$���玩�g��$\delta \bm{A}_p$�ւ̓`�d�ƍl������D
�����
\begin{align*}
\delta \bm{A}_p = \Theta_p \delta \hat{Z}_p
\end{align*}
�Ƃ����ƁC$\Theta_p$�͎��̂悤�ɍċA�I�ɋ��߂���D
�܂������̂Ɋւ��Ă�
\begin{align*}
\Theta_0 = -\hat{I}_0^{-1}
\end{align*}
�ł���D
�܂��C����$p$�ɂ���$\Theta_p$�����m�Ƃ���ƁC���̎q����$c$�ɂ���
\begin{align*}
\delta \bm{A}_c &= T_{cp} \Theta_p \, \delta \hat{Z}_p - S_{cp} \, \delta \hat{Z}_c \\
                &= (T_{cp} \Theta_p T_{cp}^\mathsf{T} - S_{cp}) \, \delta \hat{Z}_c, \\
\Theta_c        &= T_{cp} \Theta_p T_{cp}^\mathsf{T} - S_{cp}
\end{align*}
�ƂȂ�D

�M�A�ɂ��A��������ꍇ����{�I�ɂ͓��l�ł���D


\end{document}

\subsection{���͂̓`�d}

\subsection{�A�ϕ�}
\begin{tabular}{ll}
���̂̐� 													& 	$N^B$							\\
���̂̃C���f�b�N�X											&	$i \in \{1, 2, \dots, N^B\}$	\\
\\
�֐ߌn�̐�													&	$N^A$							\\
�֐ߌn�̃C���f�b�N�X										&	$j \in \{1, 2, \dots, N^A\}$	\\
�֐ߌn$j$�̊֐߂̐�											&	$N_j$							\\
�֐߂̃C���f�b�N�X											&	$j'\in \{1, 2, \dots, N_j\}$	\\
�֐ߌn$j$�̊֐�$j'$�̕ψ�									&	$\bm{q}_{jj'}$					\\
�֐ߌn�ɑ����Ȃ����̂̃C���f�b�N�X�W��						&	$\mathcal{B}_0$					\\
�֐ߌn$j$�Ɋ܂܂�鍄�̂̃C���f�b�N�X�W��					&	$\mathcal{B}_j$					\\
�֐ߌn$j$�̃��[�g���̂̃C���f�b�N�X							&	$\mathcal{B}_{j0}$				\\
�֐ߌn$j$�̊֐�$j'$�Őe���̂ƂȂ��鍄�̂̃C���f�b�N�X		&	$\mathcal{B}_{jj'}$				\\
�֐ߌn$j$�̈�ʉ����x										&	$\dot{\bm{q}}_j = [\bm{V}_{\mathcal{B}_{j0}}, \dot{\bm{q}}_{j1}, \dots, \dot{\bm{q}}_{jN_j}]$	\\
�֐ߌn$j$���獄��$i$�ւ̃��R�r�A��							&	$\hat{J}_{ji} \;(i \in \mathcal{B}_j, \, \bm{V}_i = \hat{J}_{ji} \dot{\bm{q}}_j)$	\\
\\
�S���̐�													&	$N^C$							\\
�S���̃C���f�b�N�X											&	$k = \{1, 2, \dots, N^C\}$		\\
�S��$k$����p���鍄�̂̃C���f�b�N�X�W��						&	$\mathcal{B}_k$					\\
�S��$k$�̑��x												&	$\bm{w}_k$						\\
�S��$k$�̍S����												&	$\bm{\lambda}_k$				\\
����$i$����S��$k$�ւ̃��R�r�A��							&	$J_{ki} \; (i \in \mathcal{B}_k) \;\;\; (\bm{w}_k = \sum_{i \in \mathcal{B}_k} J_{ki} \bm{V}_i)$	\\
\\
\end{tabular}

�֐ߌn�ɑ����Ȃ�����$i \in \mathcal{B}_0$�̉^����������
\begin{align}
	M_i \dot{\bm{V}}_i = \bm{F}_i + \bm{F}^\mathrm{ext}_i + \sum_{k: i \in \mathcal{B}_k} J_{ki}^\mathsf{T} \bm{\lambda}_k
\end{align}
�Ə�����D
�I�C���[�ϕ����{����
\begin{align}
	\bm{V}_i[t+1]
		&= \bm{V}_i[t] + M_i^{-1}(\bm{F}_i + \bm{F}^\mathrm{ext}_i + \sum_{k: i \in \mathcal{B}_k} J_{ki}^T \bm{\lambda}_k) \varDelta t\\
		&= \sum_{k: i \in \mathcal{B}_k}
			\underbrace{\left(M_i^{-1} J_{ki}^T \right)}_{\mbox{$T_{ki}$}}\bm{\lambda}_k \varDelta t
			 + \underbrace{\left(\bm{V}_i[t] + M_i^{-1}(\bm{F}_i + \bm{F}^\mathrm{ext}_i)\varDelta t\right)}_{\mbox{$\bm{V}^\mathrm{free}_i$}}\\
\end{align}
�ƂȂ�D
$T_{ki}$�͍S���͂��獄�̂̑��x�ω��ւ̎ʑ��C
$\bm{V}^\mathrm{free}_i$�͍S���͂���p���Ȃ��ꍇ�̎������̑��x�Ɖ��߂ł���D

����C�֐ߌn$j$�̉^����������
\begin{align}
	M_j \ddot{\bm{q}}_j &= \bm{F}_j + \bm{\tau}_j + \sum_{i \in \mathcal{B}_j} \hat{J}_{ji}^T (\bm{F}^\mathrm{ext}_i + \sum_{k : i \in \mathcal{B}_k} J_{ki}^T \bm{\lambda}_k)
\end{align}
�Ə�����D
������$M_j$�͊����s��C$\bm{F}_j$�̓R���I�����C$\bm{\tau}_j$�͊֐߃g���N�ł���D
�I�C���[�ϕ����{����
\begin{align}
	\dot{\bm{q}}_j[t+1] &= \dot{\bm{q}}_j[t]
	 + M_j^{-1}(\bm{F}_j + \bm{\tau}_j + \sum_{i \in \mathcal{B}_j} \hat{J}_{ji}^T (\bm{F}^\mathrm{ext}_i + \sum_{k: i \in \mathcal{B}_k} J_{ki}^T \bm{\lambda}_k))\varDelta t\\
		&= M_j^{-1} \bm{\tau}_j \varDelta t
		+ \sum_{i,k: i \in \mathcal{B}_j \cap \mathcal{B}_k} \left(M_j^{-1} \hat{J}_{ji}^T J_{ki}^T \right) \bm{\lambda}_k \varDelta t
		+ \left( \dot{\bm{q}}_j[t] + M_j^{-1}(\bm{F}_j + \sum_{i \in \mathcal{B}_j} \hat{J}_{ji}^T \bm{F}^\mathrm{ext}_i)\varDelta t \right)
\end{align}
�ƂȂ�D
�����ŋߎ��I��$\bm{V}_i[t+1] = \hat{J}_{ji} \dot{\bm{q}}_j[t+1]$�Ƃ���ƁC
\begin{align}
	\bm{V}_i[t+1] 
		&= \hat{J}_{ji} M_j^{-1} \bm{\tau}_j \varDelta t
		 + \sum_{i',k: i' \in \mathcal{B}_j \cap \mathcal{B}_k} \left( \hat{J}_{ji} M_j^{-1} \hat{J}_{ji'}^T J_{ki'}^T \right) \bm{\lambda}_k \varDelta t 
		 + \underbrace{\hat{J}_{ji} \left( \dot{\bm{q}}_j[t] + M_j^{-1}(\bm{F}_j + \sum_{i' \in \mathcal{B}_j} \hat{J}_{ji'}^T \bm{F}^\mathrm{ext}_{i'})\varDelta t \right)}_{\mbox{$\bm{V}^\mathrm{free}_{ji}$}}
\end{align}
�ƂȂ�D
����ɁC�S��$k$�̎���$t+1$�ł̑��x���ߎ��I��$\bm{w}_k[t+1] = \sum_{i \in \mathcal{B}_k} J_{ki} \bm{V}_i[t+1]$�Ƃ���ƁC
\begin{align}
	\bm{w}_k[t+1] &=
	\sum_{i,j:\; i \in \mathcal{B}_j \cap \mathcal{B}_k}J_{ki} \hat{J}_{ji} M_j^{-1} \bm{\tau}_j \varDelta t
	+ \sum_{k'} \left(
		\sum_{i \in \mathcal{B}_0 \cap \mathcal{B}_k \cap \mathcal{B}_{k'}} J_{ki} T_{k'i}
		 + \sum_{i,i',j:\; i \in \mathcal{B}_j \cap \mathcal{B}_k,\, i' \in \mathcal{B}_j \cap \mathcal{B}_{k'}} J_{ki} \hat{J}_{ji} M_j^{-1} \hat{J}_{ji'}^T J_{ki'}^T
	\right) \bm{\lambda}_{k'} \varDelta t \notag\\
	&+ \left( \sum_{i \in \mathcal{B}_0 \cap \mathcal{B}_k} J_{ki} \bm{V}^\mathrm{free}_i
	  + \sum_{i,j:\; i \in \mathcal{B}_j \cap \mathcal{B}_k} J_{ki} \bm{V}^\mathrm{free}_{ji} \right)
	%\\
	%&\bm{w}_k[t+1] = \sum_{i \in \mathcal{B}_k} J_{ki}
	%\left\{ \begin{aligned}
	%	\sum_{k': i \in \mathcal{B}_{k'}} T_{k'i} \bm{\lambda}_{k'} \varDelta t + \bm{V}^\mathrm{free}_i	& (i \in \mathcal{B}_0)\\
	%	\sum_{i' \in \mathcal{B}_j} \sum_{k': i' \in \mathcal{B}_{k'}} T_{k'i} \bm{\lambda}_{k'} + \bm{V}^\mathrm{free}_i	& (i \in \mathcal{B}_j)
	%\end{aligned} \right.
\end{align}

�n�S�̂̑��x�ϐ���
\begin{align}
	\bm{V} = \begin{bmatrix}\bm{V}_{\mathcal{B}_0} \\ \dot{\bm{q}}_1 \\ \vdots \\ \dot{\bm{q}}_{N^A}\end{bmatrix},\;
	\bm{\Lambda} = \begin{bmatrix}\bm{\lambda}_1 \\ \bm{\lambda}_2 \\ \vdots \\ \bm{\lambda}_{N^C}\end{bmatrix}
\end{align}

\newcommand{\Rp}{R_\mathrm{p}}
\newcommand{\rp}{\bm{r}_\mathrm{p}}
\newcommand{\Rc}{R_\mathrm{c}}
\newcommand{\rc}{\bm{r}_\mathrm{c}}

\newcommand{\Rpj}{R_\mathrm{pj}}
\newcommand{\rpj}{\bm{r}_\mathrm{pj}}
\newcommand{\Rcj}{R_\mathrm{cj'}}
\newcommand{\rcj}{\bm{r}_\mathrm{cj'}}
\newcommand{\Rjj}{R_\mathrm{jj'}}
\newcommand{\rjj}{\bm{r}_\mathrm{jj'}}

\newcommand{\vsp}{\hat{\bm{v}}}
\newcommand{\asp}{\hat{\bm{a}}}
\newcommand{\csp}{\hat{\bm{c}}}
\newcommand{\fsp}{\hat{\bm{f}}}
\newcommand{\Isp}{\hat{I}}
\newcommand{\Zsp}{\hat{Z}}
\newcommand{\Iart}{\hat{I}^A}
\newcommand{\Zart}{\hat{Z}^A}

\newcommand{\bq}{\bm{q}}
\newcommand{\bqd}{\dot{\bm{q}}}
\newcommand{\bqdd}{\ddot{\bm{q}}}
\newcommand{\btau}{\bm{\tau}}

\subsection{�A���X�g�e���X�����̉��ł�ABA}

S. McMillan and D. E. Orin, Efficient Computation
of Articulated-Body Inertias Using Successive
Axial Screws," IEEE Trans. on Robotics and
Automation, vol. 11, pp. 606{611, 1995.

Alternate formulations for the manipulator inertia matrix
IJRR 1991 vol10, no1

	\begin{tabular}{ll}
	$\Rp$		& �Î~�t���[���ɑ΂���e���̂̌��� \\
	$\rp$		& �Î~�t���[���Ɋւ���e���̂̈ʒu \\
	$\Rc$		& �Î~�t���[���ɑ΂���q���̂̌��� \\
	$\rc$		& �Î~�t���[���Ɋւ���e���̂̈ʒu \\
				& \\
	$\Rpj$		& �e���̂ɑ΂���\�P�b�g�̌��� \\
	$\rpj$		& �e���̂�
	\end{tabular}

	$R_p, R_c$��$3 \times 3$�̐��K�����s��C$r_p, r_c$��3�����x�N�g���ł���D

	%������$\prj^p, \pRj, \crj^c, \cRj$�͊e���̂Ɗ֐߂̈ʒu�֌W�����߂�萔�ł���D
	%���$\jrj, \jRj$�͊֐߂̕ό`��\��������̂ł���C�֐ߕϐ�$\bm{q}$�̊֐��ł���D

	�e�t���[���Ԃ̈ʒu�E�p���̊֌W�͎��̒ʂ�D
	\begin{align}
		\bm{r}_c &= \bm{r}_p + R_p \bm{r}_{pj} + R_p R_{pj} \bm{r}_{jj'} - R_p R_{pj} R_{jj'} R_{cj'}^T \bm{r}_{cj'} \\
		R_c &= R_p R_{pj} R_{jj'} R_{cj'}^T
	\end{align}
	�㎮�����ԂŔ�������Ǝ��̑��x�`�d�̎��𓾂�D
	\begin{align}
		\bm{v}_c &= \bm{v}_p
			 + \bm{\omega}_p \times \bm{r}_{pc}
			 + R_j \bm{v}_{jj'}
			 - (R_j \bm{\omega}_{jj'}) \times (R_c \bm{r}_{cj'}) \label{vel_prop_abs}\\
		\bm{\omega}_c &= \bm{\omega}_p + R_j \bm{\omega}_{jj'}	\label{angvel_prop_abs}
	\end{align}
	$\dot{R}R^T \bm{p} = \bm{\omega} \times \bm{p} \;\; (\bm{p}\text{:�C��})$�ɒ��ӁD
	������$R_j = R_p R_{pj}, \bm{r}_{pc} = \bm{r}_c - \bm{r}_p$�D
	$\bm{v}_p, \bm{v}_c, \bm{\omega}_p, \bm{\omega}_c$�͂��ꂼ��Î~�t���[�����猩���e�t���[������юq�t���[���̑��x�Ɗp���x�ł���D
	�܂��C$\bm{v}_{jj'}, \bm{\omega}_{jj'}$�͊֐ߓ��̓t���[�����猩���֐ߏo�̓t���[���̑��x�Ɗp���x�ł���D

	������x���������
	\begin{align}
		\bm{a}_c &= \bm{a}_p
			 + \bm{\alpha}_p \times \bm{r}_{pc}
			 + \bm{\omega}_p \times \bm{v}_{pc}
			 + \bm{\omega}_p \times R_j \bm{v}_{jj'}
			 + R_j \bm{a}_{jj'} \\
			 &- (\bm{\omega}_p \times R_j \bm{\omega}_{jj'}) \times R_c \bm{r}_{cj'}
			 - R_j \bm{\alpha}_{jj'} \times R_c \bm{r}_{cj'}
			 - R_j \bm{\omega}_{jj'} \times (\bm{\omega}_c \times R_c \bm{r}_{cj'})
		\label{acc_prop_abs}\\
		\bm{\alpha}_c &= \bm{\alpha}_p + \bm{\omega}_p \times R_j \bm{\omega}_{jj'} + R_j \bm{\alpha}_{jj'}
		\label{angacc_prop_abs}
	\end{align}
	������$\bm{a}_p, \bm{a}_c, \bm{\alpha}_p, \bm{\alpha}_c$�͂��ꂼ��Î~�t���[�����猩���e�t���[������юq�t���[���̉����x�Ɗp�����x�ł���D
	�܂��C$\bm{a}_{jj'}, \bm{\alpha}_{jj'}$�͊֐ߓ��̓t���[�����猩���֐ߏo�̓t���[���̉����x�Ɗp�����x�ł���D

	�ȏ�͐Î~�t���[���ŕ\�킵�����x�E�����x�`�d�̎��D��������[�J���t���[���\���ɕϊ�����D
	��(\ref{vel_prop_abs})(\ref{angvel_prop_abs})�ɍ�����$R_c^T$���������
	\begin{align}
		\bm{v}_c^l &= R_{cp}(\bm{v}_p^l + \bm{\omega}_p^l \times \bm{r}_{pc}^l) + R_{cj} (\bm{v}_{jj'} + \bm{\omega}_{jj'} \times R_{cj}^T (-\bm{r}_{cj'}))
		\label{vel_prop_local}\\
		\bm{\omega}_c^l &= R_{cp}\bm{\omega}_p^l + R_{cj}\bm{\omega}_{jj'}
		\label{angvel_prop_local}
	\end{align}
	�ƂȂ�D
	���Ă�����spatial transformation matrix���ȉ��̂悤�ɒ�`����D
	\begin{align}
		X(R, \bm{r}) :=
			\begin{bmatrix}R & O \\ O & R\end{bmatrix}
			\begin{bmatrix}I & O \\ \hat{\bm{r}}^T & I\end{bmatrix}
			= \begin{bmatrix}R & O \\ R\hat{\bm{r}}^T & R\end{bmatrix}
		\label{sp_mat}
	\end{align}
	$\hat{(\cdot)}$�͊O�ω��Z�Ɠ����Șc�Ώ̍s��ł���D���Ȃ킿$\hat{\bm{a}}\bm{b} = \bm{a} \times \bm{b}$�D
	����Ƒ��x�`�d(\ref{vel_prop_local})(\ref{angvel_prop_local})�͈ȉ��̂悤�ɏ�����D
	\begin{align}
		\vsp_c = X_{cp} \vsp_p + X_{cj} \vsp_{jj'}
		\label{vel_prop_sp}
	\end{align}
	������$X_{cp} = X(R_{cp}, \bm{r}_{pc}^l), \; X_{cj} = X(R_{cj}, R_{cj}^T(-\bm{r}_{cj'}))$�D
	������$\vsp_c, \vsp_p, \vsp_j$�͂��ꂼ��q�t���[���C�e�t���[���C�֐߂�spatial velocity�ł���C�����ɂ��^������D
	\begin{align}
		\vsp_c = \begin{bmatrix}\bm{\omega}_c^l \\ \bm{v}_c^l\end{bmatrix}, \;\;
		\vsp_p = \begin{bmatrix}\bm{\omega}_p^l \\ \bm{v}_p^l\end{bmatrix}, \;\;
		\vsp_{jj'} = \begin{bmatrix}\bm{\omega}_{jj'} \\ \bm{v}_{jj'}\end{bmatrix}
		\label{sp_vel}
	\end{align}

	���l�Ɏ�(\ref{acc_prop_abs})(\ref{angacc_prop_abs})�ɍ�����$R_c^T$�������Đ��������
	\begin{align}
		\asp_c = X_{cp} \asp_p + X_{cj} \asp_{jj'} + \csp_c
		\label{acc_prop_sp}
	\end{align}
	�Ə�����D
	������$\asp_c, \asp_p, \asp_{jj'}$�͂��ꂼ��q�t���[���C�e�t���[���C�֐߂�spatial acceleration�ł���C�����ɂ��^������D
	\begin{align}
		\asp_c = \begin{bmatrix}\bm{\alpha}_c^l \\ \bm{a}_c^l\end{bmatrix}, \;\;
		\asp_p = \begin{bmatrix}\bm{\alpha}_p^l \\ \bm{a}_p^l\end{bmatrix}, \;\;
		\asp_{jj'} = \begin{bmatrix}\bm{\alpha}_{jj'} \\ \bm{a}_{jj'}\end{bmatrix}
		\label{sp_acc}
	\end{align}
	�܂�$\csp_c$��spatial coriolis acceleration�ƌĂ΂�C���̒ʂ�D
	\begin{align}
		\csp_c = \begin{bmatrix}
			R_{cp}\bm{\omega}_p^l \times R_{cj}\bm{\omega}_{jj'} \\
			R_{cp}\bm{\omega}_p^l \times R_c^T \bm{v}_{pc} + R_{cp}\bm{\omega}_p^l \times R_{cj}\bm{v}_{jj'}
				- (R_{cp}\bm{\omega}_p^l \times R_{cj}\bm{\omega}_{jj'}) \times \bm{r}_{cj'}
				- R_{cj}\bm{\omega}_{jj'} \times (R_c^T \bm{\omega}_c \times \bm{r}_{cj'})
		\end{bmatrix}
		\label{sp_coriolis}
	\end{align}

	��(\ref{vel_prop})�̋t�����̑��x�ϊ��́C
	\begin{align}
		X_{ab} := X_{ba}^{-1} = \begin{bmatrix}R_{ab} & \bm{r}_{ab}^\times R_{ab} \\ O & R_{ab}\end{bmatrix}
	\end{align}
	��p����
	\begin{align}
		\bm{V}_a = X_{ab} \bm{V}_b - \begin{bmatrix}\bm{v}_{ab} + \bm{r}_{ab} \times \bm{\omega}_{ab} \\ \bm{\omega}_{ab}\end{bmatrix}
		\label{vel_prop_inverse}
	\end{align}
	�ƂȂ�D

	�㎮�ɂ���Ƃ���Ƃ���������Đ��������
	\begin{align}
		\bqdd = \left \{\btau - \left(J^T X_{cj}^T \Iart_c X_{cp}\right)\asp_p
					+ J^T X_{cj}^T \left(\Zart_c + \Iart_c \left(X_{cj} \dot{J}\bqd + \csp_c\right)\right)
				\right \}
				\left(J^T X_{cj}^T \Iart_c X_{cj} J\right)^{-1}
	\end{align}
	�ł��邩��
	\begin{align}
		\fsp_p  &= \Iart_p \asp_p + \Zart_p\\
		\Iart_p &= \Isp_p + X_{cp}^T \Iart_c X_{cp} - X_{cp}^T \Iart_c X_{cj} JJ^T X_{cj}^T \Iart_c X_{cp} (J^T X_{cj}^T \Iart_c X_{cj} J)^{-1}\\
		\Zart_p &= \Zsp_p + X_{cp}^T(\Zart_c + \Iart_c(\csp_c + X_{cj}\dot{J}\bqd))\notag\\
				& + X_{cp}^T \Iart_c X_{cj} J (\btau - J^T X_{cj}^T (\Zart_c + \Iart_c(\csp_c + X_{cj}\dot{J}\bqd)))(J^T X_{cj}^T \Iart_c X_{cj} J)^{-1}
	\end{align}
