# Computational Dynamics of Multi-Rigid-Body System in Screw Coordinate
## 2 Kinematics of a Multi-Rigid-Body System
### Relative Displacement, Velocity and Acceleration of Each Joint  
In Figure 1a, for a multi-rigid-body system, there are \(p\) kinematic chains each of which has \(n\) joints. In the multi-rigid-body system, \(L\) represents a link, \(j\) represents a joint and \(loop\) represents a kinematic chain. As shown in Figure 1b, suppose \(L_{i-1}\) and \(L_{i-2}\) are two rigid bodies in a serial kinematic chain which are connected by joint \(j_{i-1}\). The dual 3-dimensional vectors, \({}_{0}\omega_{1}^{p}\) and \({}_{0}\
u_{1}^{p}\), can fully determine the motion of link \(L_{1}^{p}\). Its velocity screw vector can be defined as  
\[{}_{0}\
u_{1}^{p}= \begin{bmatrix}\omega_{j}^{p}\\ {}_{0}\
u_{1}^{p}\end{bmatrix}  \tag{1}\]  
Figure 1: A multi-rigid-body system: (**a**) multi-rigid-body system; (**b**) a serial kinematic chain. where \({}_{0}\omega_{1}^{p}\) is the relative angular velocity vector of joint \(j_{1}^{p}\) with respect to the grounded link \(0\), \({}_{0}\omega_{1}^{p}\) is the relative linear velocity of a point on the link \(L_{1}^{p}\) with its extended body that is passing through the origin of the absolute coordinate frame at this instant. The 3-dimensional vector \({}_{0}\omega_{1}^{p}\) can be expressed by \({}_{0}\omega_{1}^{p}={}_{0}\tau_{1}^{p}\times{}_{0}\omega_{1}^{p}\) where \({}_{0}\tau_{1}^{p}\) is the position vector of joint \(j_{1}^{p}\) with respect to the origin of the grounded coordinate frame. Suppose \(\|e_{1}^{p}\|=1\), Equation (1) can be rewritten as  
\[{}_{0}\
abla_{1}^{p}={}_{0}\omega_{1}^{p} \begin{bmatrix}e_{1}^{p}\\ {}_{0}\tau_{1}^{p}\times e_{1}^{p}\end{bmatrix}  \tag{2}\]  
where \(e_{1}^{p}\) is the direction vector of joint \(j_{1}^{p}\) in the absolute coordinate frame, and \({}_{0}\omega_{1}^{p}\) is the relative angular speed of joint \(j_{1}^{p}\) in the absolute coordinate frame. Let  
\[\xi_{1}^{p}= \begin{bmatrix}e_{1}^{p}\\ {}_{0}\tau_{1}^{p}\times e_{1}^{p}\end{bmatrix}  \tag{3}\]  
where \(\xi_{1}^{p}\) is the unit screw of joint \(j_{1}^{p}\). Substituting Equation (3) into Equation (2) yields  
\[{}_{0}\
abla_{1}^{p}={}_{0}\omega_{1}^{p}\xi_{1}^{p} \tag{4}\]  
For a kinematic chain \(loop(p)\) shown in Figure 1a, based on the linear combination, the kinematics of the end joint \(j_{1}^{p}\) can be expressed as:  
\[\left\{ \begin{array}{l}{}_{0}\omega_{n}^{p}={}_{0}\omega_{1}^{p}+{}_{1} \omega_{2}^{p}+{}_{2}\omega_{3}^{p}+\cdots+{}_{n-2}\omega_{n-1}^{p}+{}_{n-1} \omega_{n}^{p}\\ {}_{0}\omega_{n}^{p}={}_{0}\omega_{1}^{p}+{}_{1}\omega_{2}^{p}+{}_{2}\omega_{3 }^{p}+\cdots+{}_{n-2}\omega_{n-1}^{p}+{}_{n-1}\omega_{n}^{p}\end{array} \right. \tag{5}\]  
In accordance with Equation (2), the forward kinematics of a single kinematic chain can be rewritten as:  
\[{}_{0}\
abla_{n}^{p}=\sum_{i=0}^{n}{}_{i}\
abla_{i+1}^{p}=S^{p}\omega^{p} \tag{6}\]  
where  
\[S^{p}= \begin{bmatrix}\xi_{1}^{p}&\xi_{2}^{p}&\cdots&\xi_{n}^{p}\end{bmatrix}  \tag{7}\]  
which represents the unit screw matrix of the single kinematic chain \(loop(p)\) and  
\[\omega^{p}= \begin{bmatrix}_{0}\omega_{1}^{p}&{}_{1}\omega_{2}^{p}&\cdots&{}_{ n-1}\omega_{n}^{p}\end{bmatrix} ^{T} \tag{8}\]  
where \({}_{n-1}\omega_{n}^{p}\) is the relative angular speed of the link \(L_{n}^{p}\) with respect to the link \(L_{n-1}^{p}\) connecting joint \(j_{n-1}^{p}\). With the definition of the twist matrix and Equation (6), the velocity screw equation of a multi-rigid-body system would be obtained,  
\[S\omega=V \tag{9}\]  
where  
\[S=\text{diag} \begin{bmatrix}S^{1}&S^{2}&\cdots&S^{p}\end{bmatrix} _{6p\times 6p} \tag{10}\]  
And  
\[\omega= \begin{bmatrix}\left(\omega^{1}\right)^{T}&\left(\omega^{2}\right)^{T}& \cdots&\left(\omega^{p}\right)^{T}\end{bmatrix} _{6p\times 1}^{T} \tag{11}\]in which \(\mathbf{\omega}^{p}\) is the vector of relative angular speeds of all joints in the single kinematic chain \(loop(p)\), and  
\[\mathbf{V}=\left[ \begin{pmatrix}0\mathbf{V}_{n}^{1}\\ 0\end{pmatrix} ^{T}\quad \begin{pmatrix}0\mathbf{V}_{n}^{2}\\ 0\end{pmatrix} ^{T}\quad\cdots\quad