A system, apparatus, and method are provided for control of a catheter (including an ablation catheter), bronchoscope/endoscope and beveled needle. Control of a bronchoscope (100) is calculated for a 3D environment based on capabilities of the bronchoscope and the patient morphology. This can be used to plan and simulate an optimal motion, train or compare surgical techniques or automate the procedure. A particular bronchoscope may be recommended based on its form and flexibility as well as based on the personal morphology of the patient rather than relying on statistical norms. For all tools, a 6 dimensional configuration space problem is solved using 3 storage dimensions and a ‘6D neighborhood’ for path planning. The present invention finds the kinematically feasible path from a ‘start’ to a goal, while avoiding obstacles and dangerous regions.