kin_planner: changed environment path, how it is found, and did some refactoring.
Updating and refactoring kin_planner and adding new step files to match new inverse kinematics functionality.