Collision Free Path Planner for Hw4 for Robotics Class