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

1/theta missing when computing G-1 ? (python & matlab) #44

Open louisdehaybe opened 2 years ago

louisdehaybe commented 2 years ago

Hello,

In the book, page 106, they provide the analytical expression for G^-1, that is being use to calculate the log_SE3 . In particular, the experssion begins by 1/theta * Identity + ....

However, in both matlab and python versions of the code, the function MatrixLog6 begins by eye(3) + ... , so there is not the 1/theta in the code.

Is there an error, or do I miss something ?

Thanks !

HuanWeng commented 1 year ago

Thank you and good catch! However the function outputs correctly. No error there.

The discrepancy between the algorithm in the book and code is due to a small trick used in the code. MatrixLog6() outputs $[{\cal S}]\theta$, the top right $3\times3$ submatrix of which is $v\theta = \theta G^{-1}(\theta)p = (\theta G^{-1}(\theta))p$. Multiplying $\theta$ and $G^{-1}$ together at first reduces some unnecessary computation.