Control And Motion Planning for Dexterous Manipulation