rotation matrix simplification

[stonneau]

This commit should prevent some annoying cases that apparently were not handled in my implementation of the rodriguez formula. It needs to be tested a bit to make sure it works well though!

It seems to work without any further modifications but right now I don't have the means to go all the way to the whole body generation and check whether this is true

[jcarpent]

Why to not simply use the formulations of Pinocchio which are proven and handle also the Taylor expansions?

[stonneau]

This method was coded before switching to pinocchio, and I was not aware it existed there. It indeed may be a good idea to reduce the code load, though I must admit I don't see the connection with the Taylor expansion ?

Could you elaborate on how that is improving the computation of the rotation matrix ?

[jcarpent]

Yes, of course. The idea of having Taylor expansion in addition to the standard formula is to handle the case where the norm of the angular velocity vector is closed to zero. In this precise case, the classic Rodrigues formula may lead to numerical issues (as you're divided some terms by this norm). You can go around this issue by computing the Taylor expansion of the normalized terms of the Rodrigues formula. This what has been implemented in Pinocchio to be robust against numerical issues.

I hope to have been clear in this presentation.

[jmirabel]

@stonneau wants a method to compute the shortest (when unique) rotation matrix that transform a vector into another. From Eigen's source code, I think Eigen's method is numerically stable (but can be slow for two vectors nearly opposite).

@jcarpent Has this been re-implemented in Pinocchio ? As far as I know, it hasn't. Of course, I may have missed it. I think you got confused with other Taylor expansions.

[jcarpent]

@jmirabel You're right. I was confused. @stonneau Sorry for the burden. @jmirabel is right, Eigen implementation seems to do what you need.

[stonneau]

thanks to both!