Driving the Tool along 3D Trajectory