# Analysis of Cost Functions for Reinforcement Learning of Reaching Tasks in Humanoid Robots
## 2 Stable Motion Imitation Framework
### Stability Control and Motion Mapping
The relationship between the motion of the center of mass and the robot's joint angles is given by the Jacobian of the center of mass \(\mathbf{J}_{CoM}\), which relates the joint velocities to the velocity of the center of mass as  
\[\dot{\mathbf{x}}_{CoM}=\mathbf{J}_{CoM}\dot{\mathbf{\theta}}. \tag{7}\]  
The Jacobian of the center of mass \(\mathbf{J}_{CoM}\in\mathbb{R}^{3\times n}\), with \(n\) being the number of degrees of freedom, is defined as follows:  
\[\mathbf{J}_{CoM}=\frac{\partial\mathbf{x}_{CoM}}{\partial\mathbf{\theta}}. \tag{8}\]Using (7), a stable whole body motion of a humanoid robot can be generated by following the desired stabilized trajectory of the center of mass. Considering that the humanoid robot has no fixed base frame, it becomes crucial to take into account the floating point dynamics and apply additional constraints to keep the robot stable. [PERSON] et al. [33] discussed how to apply constraints both in joint and in Cartesian space. We are interested in generating whole-body motion while ensuring contact with the ground on both feet of the robot. Thus, it makes sense to constrain the poses of both feet to remain stationary and maintain contact with the ground. These constraints are expressed as follows:  
\[\mathbf{0}= \begin{bmatrix}\dot{\mathbf{x}}_{LF}\\ \omega_{LF}\end{bmatrix} =\mathbf{J}_{LF}\dot{\mathbf{\theta}}, \tag{9}\]  
\[\mathbf{0}= \begin{bmatrix}\dot{\mathbf{x}}_{RF}\\ \omega_{RF}\end{bmatrix} =\mathbf{J}_{RF}\dot{\mathbf{\theta}}, \tag{10}\]  
where \(\dot{\mathbf{x}}_{LF}\) and \(\omega_{LF}\) are the linear and angular velocity of the left foot, while \(\dot{\mathbf{x}}_{RF}\) and \(\omega_{RF}\) denote the linear and angular velocity of the right foot. \(\mathbf{J}_{LF}\) and \(\mathbf{J}_{RF}\in\mathbb{R}^{6\times n}\), expressed in the fixed world coordinate system, are the left and right foot Jacobians, respectively. They establish the relationship between the joint velocities and the linear and angular velocities of the feet. Finally, by combining the stable desired trajectory of the robot's center of mass and the constant pose of the robot's feet, we can generate joint velocities of stable motion using the following relationship:  
\[ \begin{bmatrix}\dot{\mathbf{x}}_{CoM}\\ \mathbf{0}\\ \mathbf{0}\end{bmatrix} =\mathbf{\tilde{J}}\dot{\mathbf{\theta}},\ \mathbf{\tilde{J}}= \begin{bmatrix}\mathbf{J}_{CoM}\\ \mathbf{J}_{LF}\\ \mathbf{J}_{RF}\end{bmatrix} . \tag{11}\]  
Using Equation (11) and the desired velocity of the center of mass calculated by integrating Equations (5) and (6), we compute the joint velocities that preserve the stability of the robot  
\[\dot{\mathbf{\theta}}=\mathbf{J}^{\dagger} \begin{bmatrix}\dot{\mathbf{x}}_{CoM}\\ \mathbf{0}\\ \mathbf{0}\end{bmatrix} , \tag{12}\]  
where \(\mathbf{\tilde{J}}^{\dagger}\) is the Moore-Penrose pseudoinverse of \(\mathbf{\tilde{J}}\). Furthermore, the redundancy of the humanoid robot allows for an implementation of multi-task inverse kinematics alongside the stability control. Thus, we can implement stable motion imitation by solving two tasks, of which the primary task is stability control and the secondary task is motion imitation executed in the null space of the primary task. This results in the following control strategy:  
\[\dot{\mathbf{\theta}}=\mathbf{\tilde{J}}^{\dagger} \begin{bmatrix}\dot{\mathbf{x}}_{CoM} \\ \mathbf{0}\\ \mathbf{0}\end{bmatrix} +\mathbf{N}\dot{\mathbf{\theta}}_{\text{processed}}, \tag{13}\]  
where \(\mathbf{N}=(\mathbf{I}-\mathbf{\tilde{J}}^{\dagger}\mathbf{\tilde{J}})\) is the null space projection matrix of \(\mathbf{\tilde{J}}\) and \(\dot{\mathbf{\theta}}_{\text{processed}}\) are the joint velocities acquired from the demonstrated motion and processed with the safety modules. Figure 1 shows the process of obtaining a stable reproduction of human demonstrations. Utilizing Equation (13), we can generate stable joint trajectories demonstrated by a human.