Current joint state info problem
Can compute straight line cartesian path from pre-grip position, also smoothing