Contact-Aware Safe Robotic Manipulation - Dexterity and Trajectory Planning