Fix #139
[quaternion][finite differencing] add asserts to check that num diff on actionmodel does not do finite differencing on the CostModelState when the difference between state and reference is pi. If ref-x is pi, the finite differencing of a quaternion is not stabel