Adding getPose implementation to cartesian position controller
Adding getPose implementation to cartesian position controller
Made changes to calculate position error, with ik to figure out joint position from cartesian
Massive changes, fixed pid controller, and basically everything else.