For lines like this: https://github.com/ros-planning/moveit2/blob/06e5a0b29107432c8a42e1986d356193383347b8/moveit_ros/moveit_servo/src/servo_calcs.cpp#L783 h/t to @kylc Reference: https://dmpeli.math.mcmaster.ca/Matlab/Math4Q3/NumMethods/Lecture3-1.html