# Calculation of the Center of Mass Position of Each Link of Multibody Biped Robots
## 2 First Validated Theoretical Formulation
### Equilibrium  
Considering the robot equilibrium \((m_{2}\overset{\rightarrow}{\textbf{{a}}}_{2}=0;\overset{\rightarrow}{ \textbf{{M}}}_{P}=0)\) with respect to the Point P as shown in the Figure 1, Equations (1) and (2) can be modified. Moving the point of view from the vector shape to the scalar one, the values of the three components \(x\), \(y\), and \(z\) of the force and the torque can be obtained. The new system consists of six equations (five linearly independent) in six unknown values \(F_{X2}\), \(F_{Y2}\), \(F_{Z2}\), \(X_{2}\), \(Y_{2}\), \(Z_{2}\). The forces and torques in Points 0 and 1 can be calculated by means of the load cells. The torques \(M_{X2}\), \(M_{Y2}\), \(M_{Z2}\) are determined using (3) and (4).  
In order to simplify the system, the robot is positioned in two different configurations. The two configurations are chosen in order to have a simplified geometry using \(l=q\), \(n=o=e=f=0\) (see Figure 1) obtaining \(\overset{\rightarrow}{\textbf{{p}}}_{0}=[0,-l,0]^{T}\) and \(\overset{\rightarrow}{\textbf{{p}}}_{1}=[0,l,0]^{T}\).  
In the first step the robot is placed on a walking surface and the platform should be kept in a first balance configuration (Scheme A, see Figure 2), allowing a measurement of the forces \((\overset{\rightarrow}{\textbf{{F}}}_{0},\overset{\rightarrow}{\textbf{{F}}}_ {1})\) and the torques \((\overset{\rightarrow}{\textbf{{M}}}_{0},\overset{\rightarrow}{\textbf{{M}}}_ {1})\) by means of the force-torque sensors on the feet, and of the armature currents (\(\sum M_{j\_roll},\sum M_{j\_pitch},\sum M_{j\_uav}\)). In a second step, the robot is placed in a second balance configuration (Scheme B, see Figure 2), and in the same way, forces, torques, and motor currents associated with this new balance configuration are measured. The two balance configurations can be performed as the reader prefers underlining that the robot should be in a balance position. In particular, the coordinates \(X_{2A}\) and \(Z_{2A}\) are relative to the position of the center of mass of the platform in the configuration A along the first straight common line, which is chosen (in this paper) orthogonal to the plane of standing, and then parallel to the \(Z\) axis. \(m_{u}\), \(r_{u}\), \(l_{u}\), are the mass and the position of the center of mass of the robot ankle link from the floor to the ankle joint; \(U\) is the length of the ankle. \(m_{w}\), \(r_{w}\), \(l_{w}\), are the mass and the position of the center of mass of the remaining links of the platform. In the second balance configuration B, the components of the body are aligned according to a second straight common line, inclined to the vertical line with an angle \(\theta_{l}\). While \(m_{u}\), \(m_{w}\), \(r_{u}\), \(l_{u}\) remain constant, \(r_{w}\) and \(l_{w}\) change their values. In this case, \(X_{2B}\) and \(Z_{2B}\) identify the coordinates of the center of mass of the body in the second balance configuration B. The coordinates of the two feet are the same because we chose this configuration as input. The implementations have been done positioning the robot in this initial position using a leveller and the encoders of the motors.  
Based on the choice \(l=q\), \(n=o=e=f=0\) (\(\overrightarrow{p}_{0}=[0,-l,0]^{T}\) and \(\overrightarrow{p}_{1}=[0,l,0]^{T}\)), the Equations (1) and (2) can be rewritten in a general form that is function of the balance \(i\) configuration (\(l=A\) or \(l=B\)). Thus, Equations from (5) to (9) are obtained.  
\[F_{X2i}=-F_{X0i}-F_{X1i} \tag{5}\]  
\[F_{Y2i}=-F_{Y0i}-F_{Y1i} \tag{6}\]  
\[F_{Z2i}=m_{2}\cdot g-F_{Z0i}-F_{Z1i} \tag{7}\]  
\[X_{2i}=-(M_{Y0i}+M_{Y1i}+M_{Y2i})/(F_{Z0i}+F_{Z1i}) \tag{8}\]  
\[+[(F_{X0i}+F_{X1i})/(F_{Z0i}+F_{Z1i})]\cdot Z_{2i}\]  
\[Y_{2i}=[(M_{X0i}+M_{X1i}+M_{X2i})/(F_{Z0i}+F_{Z1i})]\]  
\[+[(f_{Z1i}-F_{Z0i})/(F_{Z0i}+F_{Z1i})]+ \tag{9}\]  
\[+[(F_{Y0i}+F_{Y1i})/(F_{Z0i}+F_{Z1i})]\cdot Z_{2i}\]