Trajectory Planning and Control for Robotic Manipulation of Magnetic Capsules