crv2rotation

Given a Cartesian rotation vector, \(\boldsymbol{\Psi}\), the function produces the rotation matrix required to rotate a vector according to \(\boldsymbol{\Psi}\).

The rotation matrix is given by

\[\mathbf{R} = \mathbf{I} + \frac{\sin||\boldsymbol{\Psi}||}{||\boldsymbol{\Psi}||} \tilde{\boldsymbol{\Psi}} + \frac{1-\cos{||\boldsymbol{\Psi}||}}{||\boldsymbol{\Psi}||^2}\tilde{\boldsymbol{\Psi}} \tilde{\boldsymbol{\Psi}}\]

To avoid the singularity when \(||\boldsymbol{\Psi}||=0\), the series expansion is used

\[\mathbf{R} = \mathbf{I} + \tilde{\boldsymbol{\Psi}} + \frac{1}{2!}\tilde{\boldsymbol{\Psi}}^2.\]

param psi:

Cartesian rotation vector \(\boldsymbol{\Psi}\).

type psi:

np.array

returns:

equivalent rotation matrix

rtype:

np.array

References

Geradin and Cardona, Flexible Multibody Dynamics: A finite element approach. Chapter 4