Planning and Perception for Robotic Manipulators in Semi-Structured Environments