ik working
pr2_sim.cpp, DepthSensor and KinectSensor all remaining