# Analysis of Cost Functions for Reinforcement Learning of Reaching Tasks in Humanoid Robots
## 3 Motion Adaptation by Reinforcement Learning
### Cost Function  
The performance of PI\({}^{2}\) algorithm depends on the selected cost function. One of the crucial challenges in reinforcement learning is the design of the cost function with respect to which the robot motion is optimized. The cost object function typically defines the goal and shapes the robot behavior with respect to the requirements of the task. We designed a cost function that addresses the requirements of the reaching behavior  
\[C(\tau)=w_{stab}C_{stab}(\tau)+w_{pos}C_{pos}(\tau)+w_{ori}C_{ori}(\tau)+w_{ traj}C_{traj}(\tau)+w_{acc}C_{acc}(\tau). \tag{14}\]  
The overall cost function is a weighted sum of objective functions that, respectively, address the stability of the robot (\(C_{stab}\)), accuracy of reaching motion in terms of final position and orientation (\(C_{pos},C_{ori}\)), the similarity to the initial demonstration (\(C_{traj}\)), and the smoothness of the reaching motion (\(C_{acc}\)). Our focus is on independently assessing the effect of the cost function weights, and as such, we assign a distinct weight to each individual cost term. Having explicit weights for each term allows us to conduct separate analysis of their respective contributions. It is worth noting that while the absolute scaling of the cost function weights does not affect the optimal solution, maintaining their relative scaling is crucial. The cost function terms in Equation (14) are defined as follows:  
\[C_{stab}(\tau) = \frac{1}{N+1}\sum_{i=0}^{N}f_{stab}(\mathbf{x}_{ZMP}(t_{i})), \tag{15}\] \[C_{pos}(\tau) = ||\mathbf{x}_{ee}(T)-\mathbf{g})||,\] (16) \[C_{ori}(\tau) = 2||\log(\mathbf{q}(T)*\mathbf{\tilde{q}}_{g})||,\] (17) \[C_{traj}(\tau) = \frac{1}{N+1}\text{DTW}(\tau,\tau_{demonstrated}),\] (18) \[C_{acc}(\tau) = \frac{1}{N+1}\sum_{i=0}^{N}||\tilde{\tau}(t_{i})||^{2}, \tag{19}\]where  
\[f_{stab}(\mathbf{x})= \begin{cases}\left(\frac{\text{dist}_{SP}(\mathbf{x}_{CSP})}{\text{ dist}_{SP}(\mathbf{x})}-1\right)^{2},&\text{for $\mathbf{x}\in\text{Support Polygon}$}\\ &\text{and dist}_{SP}(\mathbf{x})\geq\varepsilon>0,\\ \left(\frac{\text{dist}_{SP}(\mathbf{x}_{CSP})}{\epsilon}-1\right)^{2},&\text{ otherwise.}\end{cases}  \tag{20}\]  
Here, \(\text{dist}_{SP}\) denotes the function that computes the Euclidean distance of the point \(\mathbf{x}\) from the border of the support polygon and \(\mathbf{x}_{CSP}\) is the constant center of the support polygon, i.e., the point within the support polygon that is the furthest away from the boundary. Note that  
\[0\leq\left(\frac{\text{dist}_{SP}(\mathbf{x}_{CSP})}{\text{dist}_{SP}(\mathbf{x})}-1 \right)^{2}<\infty,\ \forall\mathbf{x}\in\text{Support Polygon}. \tag{21}\]  
The cost function (21) is equal to zero if \(\mathbf{x}\) coincides with the center of the support polygon and tends towards infinity as the ZMP approaches the border of the suppport polygon. However, we defined \(f_{stab}(\mathbf{x})\) so that the cost function is bounded when \(\mathbf{x}\) approaches the \(\varepsilon\)-boundary of the support polygon or when the measured \(\mathbf{x}\) is outside of the support polygon. We further assume that the feet are stationary and fixed to the ground. The goal position cost function (16) calculates the Euclidean distance between the robot's end-effector position and the desired position at the end of the reaching motion. Here, \(T\) is the final time step of the current rollout. The goal orientation cost function (17) uses the logarithmic map to calculate the distance metric on \(\text{S}^{3}\) between the two orientations represented by the quaternions of the desired orientation and the orientation of the end-effector at the end of the trajectory [39]. The trajectory similarity cost (18) is calculated as the dynamic time warping distance between the performed trajectory and the initially demonstrated trajectory. Additionally, we added a smoothing term (19) that minimizes the joint accelerations arising during the execution of each rollout.