# Adaptive Compensation for Robotic Joint Failures Using Partially Observable Reinforcement Learning
## 3 Methodology
### Problem Formulation
#### 3.1.3 Reward Function  
**The distance reward** is designed to encourage the robot to minimize the distance between the robot's gripper and the drawer's handle. It is calculated as follows:  
\[d=\|\mathbf{p}_{\text{gripper}}-\mathbf{p}_{\text{drawer}}\|_{2}, \tag{4}\]  
\[r_{\text{dist}}=\frac{1}{1+d^{2}}, \tag{5}\]  
where \(\mathbf{p}_{\text{gripper}}\) is the position of the robot's grasp, \(\mathbf{p}_{\text{drawer}}\) is the position of the drawer's handle, and \(\|\cdot\|_{2}\) denotes the Euclidean distance.  
**The rotation reward** is designed to align the robot's gripper orientation with the drawer's handle. It is calculated using the dot products of the forward and up axes:  
\[\text{dot}_{1}=(\mathbf{a}_{1}\cdot\mathbf{a}_{2}) \tag{6}\]  
\[\text{dot}_{2}=(\mathbf{a}_{3}\cdot\mathbf{a}_{4}), \tag{7}\]  
where \(\mathbf{a}_{1}\) and \(\mathbf{a}_{2}\) are the gripper forward axis of the Franka and the inward axis of the drawer, respectively. \(\mathbf{a}_{3}\) and \(\mathbf{a}_{4}\) are the gripper upper axis and the drawer, respectively. Eventually, the reward for matching the orientation of the hand to the drawer is expressed as follows:  
\[r_{\text{rot}}=0.5\Big{(}\text{sign}(\text{dot}_{1})\times\text{dot}_{1}^{2}+ \text{sign}(\text{dot}_{2})\times\text{dot}_{2}^{2}\Big{)}. \tag{8}\]  
**The around-handle reward** ensures that the robot's fingers are positioned appropriately around the drawer's handle. If the left finger of the Franka is above the drawer handle and the right below the drawer, we bonus a value of 0.5 for the reward function.  
\[r_{\text{handle}}=\begin{cases}0.5&\text{if }\mathbf{p}_{\text{left,finger},z}> \mathbf{p}_{\text{drawer},z}\text{ and }\mathbf{p}_{\text{right,finger},z}<\mathbf{p}_{\text{drawer},z}\\ 0&\text{otherwise},\end{cases} \tag{9}\]  
where \(\mathbf{p}_{\text{left,finger},z}\) and \(\mathbf{p}_{\text{right,finger},z}\) are the z coordinates of the left and right fingers, respectively. \(\mathbf{p}_{\text{drawer},z}\) is the z coordinate of the drawer's grasp position.  
**The open reward** is designed to encourage the robot to open the drawer, which means how far the cabinet has been opened out.  
\[r_{\text{open}}=\text{pos}_{\text{drawer\_top}}\times r_{\text{handle}}+ \text{pos}_{\text{drawer\_top}}, \tag{10}\]  
where \(\text{pos}_{\text{drawer\_top}}\) is the position of the drawer that we want the robot to open.  
**The overall reward function** is a combination of the above components scaled by their respective factors:  
\[r=w_{\text{dist}}\cdot r_{\text{dist}}+w_{\text{rot}}\cdot r_{\text{rot}}+w_{ \text{handle}}\cdot r_{\text{handle}}+w_{\text{open}}\cdot r_{\text{open}}, \tag{11}\]  
where \(w_{\text{dist}},w_{\text{rot}},w_{\text{handle}},w_{\text{open}}\) are the scaling factors for the distance reward, rotation reward, around-handle reward, and open reward, respectively.  
This reward structure ensures that the robot is incentivized to minimize the distance to the drawer handle, align its gripper orientation correctly, position its fingers around the handle, and ultimately open the drawer.