search

NxRLab / ModernRobotics

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.
http://modernrobotics.org/
MIT License
1.9k stars 807 forks source link

NaNs appear in certain cases when using inverse kinematics #39

Open 64-B1T opened 3 years ago

64-B1T commented 3 years ago

(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))

HuanWeng commented 2 years ago

Thank you for pointing this out. Looks like you've fixed that in your branch. I will check and fix that in our next update.