forward kinematics created and mostly implemented
down to construction of visited set functions/structures
code complete. no confirmation on whether it compiles
astar compiles but still requires rigorous testing