# Calculation of the Center of Mass Position of Each Link of Multibody Biped Robots
## 2 First Validated Theoretical Formulation  
### Dynamics of Multibody Biped Robots  
Figure 1 shows global and local reference Cartesian system (respectively _G-XYZ_ and _P-XpYpZp_ and three points in the space (0, 1 and 2). The three points can be considered as belonging to a rigid body in the space; furthermore, the rigid body can be compared to a humanoid platform, or multibody robot, with its center of gravity in the Point 2 and its feet in the Points 0 and 1. A humanoid robot is indeed composed of a trunk and articulated kinematic chains such as legs and arms, connected to the trunk with joints and motors, and with force-torque sensors positioned on the feet and on the hands. The Points 0 and 1 represent the feet, where the force-torque sensors are positioned, and are shown with a light blue colour; the center of mass represented with the Point 2 is shown in red; the other black points indicate the center of mass of the links of the robot. The dynamics of the system is described by the two equations (see Figure 1):  
\[m_{2}\cdot\overset{\rightarrow}{\boldsymbol{a}}_{2}=m_{2}\cdot \overset{\rightarrow}{\boldsymbol{g}}+\overset{\rightarrow}{\boldsymbol{F}}_{0 }+\overset{\rightarrow}{\boldsymbol{F}}_{1}+\overset{\rightarrow}{ \boldsymbol{F}}_{2} \tag{1}\] \[\overset{\rightarrow}{\boldsymbol{M}}_{P}=\overset{\rightarrow}{ \boldsymbol{M}}_{0}+\overset{\rightarrow}{\boldsymbol{M}}_{2}+\overset{ \rightarrow}{\boldsymbol{M}}_{1}+\overset{\rightarrow}{\boldsymbol{p}}_{0} \times\overset{\rightarrow}{\boldsymbol{F}}_{0}+\overset{\rightarrow}{ \boldsymbol{p}}_{1}\times\overset{\rightarrow}{\boldsymbol{F}}_{1}+ \overset{\rightarrow}{\boldsymbol{p}}_{2}\times\overset{\rightarrow}{ \boldsymbol{F}}_{2} \tag{2}\]  
where: \(\overset{\rightarrow}{\boldsymbol{a}}_{2}=[a_{X2},a_{Y2},a_{Z2}]^{T}\) is the acceleration of the CoM; \(m_{2}\) is the total mass of the robot without feet; \(\overset{\rightarrow}{\boldsymbol{p}}_{0}=[-n,-l,e]^{T}\) and \(\overset{\rightarrow}{\boldsymbol{p}}_{1}=[o,q,f]^{T}\) are the position vectors shown in Figure 1. \(\overset{\rightarrow}{\boldsymbol{p}}_{2}=[X_{2},Y_{2},Z_{2}]^{T}\) is the CoM position that will be determined with the proposed formula in Section 3. Figure 1: Scheme for determining the total CoM (Center of Mass) position. The direction of the force and torque vectors is only indicative. Points 0 and 1 represent the feet, where the force/torque sensors are positioned; the center of mass is represented with the Point 2; the other black points indicate the mass of the links of the robot. \(\overset{\rightarrow}{\textbf{{M}}}_{0}=[M_{X0},M_{Y0},M_{Z0}]^{T},\overset{ \rightarrow}{\textbf{{M}}}_{1}=[M_{X1},M_{Y1},M_{Z1}]^{T},\overset{ \rightarrow}{\textbf{{M}}}_{2}=[M_{X2},M_{Y2},M_{Z2}]^{T},\overset{ \rightarrow}{\textbf{{F}}}_{0}=[F_{X0},F_{Y0},F_{Z0}]^{T},\)\(\overset{\rightarrow}{\textbf{{F}}}_{1}=[F_{X1},F_{Y1},F_{Z1}]^{T},\overset{ \rightarrow}{\textbf{{F}}}_{2}=[F_{X2},F_{Y2},F_{Z2}]^{T}\) respectively represent the torques \((\overset{\rightarrow}{\textbf{{M}}}_{0},\overset{\rightarrow}{\textbf{{M}}}_ {1},\overset{\rightarrow}{\textbf{{M}}}_{2})\) and the forces \((\overset{\rightarrow}{\textbf{{F}}}_{0},\overset{\rightarrow}{\textbf{{F}}}_ {1},\overset{\rightarrow}{\textbf{{F}}}_{2})\) acting at the Points 0, 1 and 2. \(\overset{\rightarrow}{\textbf{{M}}}_{P}=[M_{XP},M_{YP},M_{ZP}]^{T}\) is the resultant moment calculated with respect to the local Cartesian system (see Figure 1). The direction of the force and torque vectors, shown in Figure 1, is only indicative; the positive direction of the force and torque vectors has been considered with the same positive direction of the global reference Cartesian system (G-XYZ). Furthermore, we can say that:  
\[\overset{\rightarrow}{\textbf{{M}}}_{2}=\left[ \begin{array}{c}M_{X2}\\ M_{Y2}\\ M_{Z2}\end{array} \right]=\left[ \begin{array}{c}\sum M_{i\_roll}\\ \sum M_{i\_pitch}\\ \sum M_{j\_uav}\end{array} \right] \tag{3}\]  
\(M_{2}\) includes only the internal torques; \(M_{0}\) and \(M_{1}\) include the ground reaction torques.