Minor updates to improve numerical stability and usability.
Minor updates to improve numerical stability and usability.

WPI-ARC/arc_utilities,WPI-ARC/arc_utilities,UM-ARM-Lab/arc_utilities,UM-ARM-Lab/arc_utilities,UM-ARM-Lab/arc_utilities,WPI-ARC/arc_utilities