# Segmented Hybrid Impedance Control for Hyper-Redundant Space Manipulators
## 5 Object Manipulation Control
### Object Position Control  
The previous section detailed the manipulator's position and force controller. The position PD controller is specifically designed to accurately achieve the desired positions; however, the manipulator is required to navigate around a non-cooperative object, which, for the sake of simplicity and to focus on controller development, is represented as a cylinder. The presence of this substantial mass introduces forces that the position controller alone cannot effectively manage, rendering it insufficient in this context. Consequently,there is a need for an additional control algorithm to complement the existing position controller, ensuring more robust handling of the dynamic interactions with the obstacle. The trajectory planning needs to ensure that the obstacle will follow a preplanned trajectory. Given the assumption that the contact point between the closest link, called the virtual end-effector, and the obstacle does not change due to the applied force by the end effector, we can calculate a trajectory for our obstacle using a fifth-order polynomial function. A trajectory is calculated for the manipulator, which, of course, can be transformed to the obstacle trajectory by:  
\[\mathrm{P_{O}}=\mathrm{T_{C_{j}}}\Bigg{[} \begin{matrix}\mathrm{P_{offset}}+ \mathrm{d}\\ 1\end{matrix} \Bigg{]}, \tag{37}\]  
where \(\mathrm{P_{O}}\) is the position vector of the obstacle's center in the global frame, \(\mathrm{T_{C_{j}}}\) is the transformation matrix corresponding to closest joint, \(\mathrm{P_{offset}}\) is the position vector of the contact point and \(\mathrm{d}\) is the fixed offset vector from the contact point to the obstacle center. Let \(\mathrm{T_{b,i}}=(\mathrm{p_{b,i}},\mathrm{R_{b,i}})\in\mathrm{SE}(3)\) be the initial pose of the manipulator, with \(\mathrm{p_{b,i}}\), \(\mathrm{R_{b,i}}\) being the initial position and orientation, respectively, of \(\mathrm{C_{j}}\). Let \(\mathrm{T_{b,f}}=\left(\mathrm{p_{b,f}},\mathrm{R_{b,f}}\right)\in\mathrm{SE }(3)\), with \(\mathrm{p_{b,f}}\), \(\mathrm{R_{b,f}}\) being the final position and orientation, respectively, of \(\mathrm{C_{j}}\). The desired E-E trajectory in each instant of time \(\mathrm{t}\in[\mathrm{t_{i}},\mathrm{t_{f}}]\) is given by:  
\[\mathrm{0}\mathrm{P_{b,d}}(\mathrm{t})=\mathrm{p_{b,i}}+\frac{\mathrm{s}( \mathrm{t})}{\left\lVert\mathrm{p_{b,f}}-\mathrm{p_{b,i}}\right\rVert}( \mathrm{P_{b,f}}-\mathrm{P_{b,i}}) \tag{38}\]  
\[\mathrm{\dot{q}}(\mathrm{t})= \begin{bmatrix}\mathrm{\dot{P}_{b,d}}\\ \mathrm{\dot{\omega}_{b,d}}\end{bmatrix} \mathrm{MJ}^{\mathrm{T}}\Big{(} \mathrm{JM}^{\mathrm{T}}\Big{)}^{\mathrm{\dagger}}, \tag{39}\]  
With \(\mathrm{MJ}^{\mathrm{T}}\Big{(}\mathrm{JM}^{\mathrm{T}}\Big{)}^{\mathrm{ \dagger}}\) representing the dynamically consistent pseudoinverse Jacobian, and \(\mathrm{\dot{p}_{b,d}}\), \(\mathrm{\dot{\omega}_{b,d}}\) representing the desired velocity and angular velocity (twist) of \(\mathrm{C_{j}}\). The algorithm for obstacle position control is based on the principles of artificial potential fields. This approach employs both repulsive and attractive forces to facilitate collision-free trajectories. In our implementation, we generate forces that actively push and pull the manipulator toward the desired position. The newly defined attractive and repulsive forces are designed to counteract the forces generated by the obstacle, ensuring effective navigation in dynamic environments. These forces are translated to the manipulator as stabilization torque. We propose a directional decomposition approach where the stabilizing forces are computed separately for each spatial direction. The stabilization force for each dimension is then computed using a proportional-exponential control approach, which provides a smooth transition between zero force and the necessary corrective force as the deviation increases.