Safe and Efficient Motion Planning in Robotic Manipulation through Formal Methods