Addresses #212 (finally) by adding const ref arguments to most utility functions in math_utils, ros_utils, and QuadKD. Not a huge effect on overall performance (interpRobotState takes about 1us instead of 5us), but nice to have. May help other controllers that require more kinematics queries.
Addresses #212 (finally) by adding const ref arguments to most utility functions in math_utils, ros_utils, and QuadKD. Not a huge effect on overall performance (interpRobotState takes about 1us instead of 5us), but nice to have. May help other controllers that require more kinematics queries.