Whole-Body Control with (Self) Collision Avoidance using Vector Field Inequalities

This work uses vector field inequalities (VFI) to prevent robot self-collisions and collisions with the workspace. Differently from previous approaches, the method is suitable for both velocity and torque-actuated robots. We propose a new distance function and its corresponding Jacobian in order to generate a VFI to limit the angle between two Pl\"ucker lines. This new VFI is used to prevent both undesired end-effector orientations and violation of joints limits. The proposed method is evaluated in a realistic simulation and on a real humanoid robot, showing that all constraints are respected while the robot performs a manipulation task.