Debugging weighted matrix, added input velocity limits
Renamed alpha to null_space_vel_gain
Null space and weighted least norm solution both almost working
Null space gain automatically reduces when EE is close to solution. Removed original KDL calculations
Cleanup debug output