Refactor utility functions for efficiency

[jcnorby]

Many of the utility functions defined in ros_utils, math_utils, and quad_kd pass inputs by value rather than const reference. Switching these should help efficiency across the board (particularly when mapping between data structures or computing FK/IK across trajectories).

[jcnorby]

Also any functions which can be defined as const should be.