Code for path-planning and custom-shaped tool computation

Summary
Code for path-planning and custom-shaped tool computation