Modern Robotics: Mechanics, Planning, and Control Code Library --- The primary purpose of the provided software is to be easy to read and educational, reinforcing the concepts in the book. The code is optimized neither for efficiency nor robustness.
(Python) In certain transforms, IK will fail due to NANs cropping up. This is because of floating point inaccuracies being fed into np.arccos. (For example in MatrixLog6)
E.g. (-1.0000000002) which will return NaN
To fix this, for each np.arccos(x) in the MR code, replace with np.arccos(np.clip(x,-1,1))
(Python) In certain transforms, IK will fail due to NANs cropping up. This is because of floating point inaccuracies being fed into np.arccos. (For example in MatrixLog6) E.g. (-1.0000000002) which will return NaN To fix this, for each np.arccos(x) in the MR code, replace with np.arccos(np.clip(x,-1,1))