Source code for rotmat

"""
Routines for working with rotation matrices
"""

"""
comment
author :  Thomas Haslwanter
date :    April-2018
"""

import numpy as np
import sympy
from collections import namedtuple

# The following construct is required since I want to run the module as a script
# inside the skinematics-directory
import os
import sys

file_dir = os.path.dirname(__file__)
if file_dir not in sys.path:
sys.path.insert(0, file_dir)

# For deprecation warnings
#import deprecation
#import warnings
#warnings.simplefilter('always', DeprecationWarning)

[docs]def stm(axis='x', angle=0, translation=[0, 0, 0]):
"""Spatial Transformation Matrix

Parameters
----------
axis : int or str
Axis of rotation, has to be 0, 1, or 2, or 'x', 'y', or 'z'
angle : float
rotation angle [deg]
translation : 3x1 ndarray
3D-translation vector

Returns
-------
STM : 4x4 ndarray
spatial transformation matrix, for rotation about the specified axis,
and translation by the given vector

Examples
--------
>>> rotmat.stm(axis='x', angle=45, translation=[1,2,3.3])
array([[ 1.        ,  0.        ,  0.        ,  1.        ],
[ 0.        ,  0.70710678, -0.70710678,  2.        ],
[ 0.        ,  0.70710678,  0.70710678,  3.3       ],
[ 0.        ,  0.        ,  0.        ,  1.        ]])
>>> R_z = rotmat.stm(axis='z', angle=30)
>>> T_y = rotmat.stm(translation=[0, 10, 0])

"""
axis = _check_axis(axis)

stm = np.eye(4)
stm[:-1,:-1] = R(axis, angle)
stm[:3,-1] = translation
return stm

[docs]def stm_s(axis='x', angle='0', transl='0,0,0'):
"""
Symbolic spatial transformation matrix about the given axis, by an angle with
the given name, and translation by the given distances.

Parameters
----------
axis : int or str
Axis of rotation, has to be 0, 1, or 2, or 'x', 'y', or 'z'
angle : string
Name of rotation angle, or '0' for no rotation,
'alpha', 'theta', etc. for a symbolic rotation.
transl : string
Has to contain three names, for the three translation distances.
'0,0,0' would correspond to no translation, and
'x,y,z' to an arbitrary translation.

Returns
-------
STM_symbolic : corresponding symbolic spatial transformation matrix

Examples
--------

>>> Rz_s = STM_s(axis='z', angle='theta', transl='0,0,0')

>>> Tz_s = STM_s(axis=0, angle='0', transl='0,0,z')

"""
axis = _check_axis(axis)

# Default is the unit matrix
STM_s = sympy.eye(4)

if angle != '0':
STM_s[:3,:3] = R_s(axis, angle)

transl = transl.replace(' ', '')
if not transl==('0,0,0'):
trans_dir = transl.split(',')
assert(len(trans_dir)==3)
for (ii, magnitude) in enumerate(trans_dir):
if magnitude != '0':
symbol = sympy.Symbol(magnitude)
STM_s[ii,-1] = symbol

return STM_s

[docs]def R(axis='x', angle=90) :
"""Rotation matrix for rotation about a cardinal axis.
The argument is entered in degree.

Parameters
----------
axis : int or str
Axis of rotation, has to be 0, 1, or 2, or 'x', 'y', or 'z'
angle : float
rotation angle [deg]

Returns
-------
R : rotation matrix, for rotation about the specified axis

Examples
--------
>>> rotmat.R(axis='x', angle=45)
array([[ 1.        ,  0.        ,  0.        ],
[ 0.        ,  0.70710678, -0.70710678],
[ 0.        ,  0.70710678,  0.70710678]])

>>> rotmat.R(axis='x')
array([[ 1.        ,  0.        ,  0.        ],
[ 0.        ,  0.        , -1.        ],
[ 0.        ,  1.        ,  0.       ]])

>>> rotmat.R('y', 45)
array([[ 0.70710678,  0.        ,  0.70710678],
[ 0.        ,  1.        ,  0.        ],
[-0.70710678,  0.        ,  0.70710678]])

>>> rotmat.R(axis=2, angle=45)
array([[ 0.70710678, -0.70710678,  0.        ],
[ 0.70710678,  0.70710678,  0.        ],
[ 0.        ,  0.        ,  1.        ]])
"""

axis = _check_axis(axis)

# convert from degrees into radian:

if axis == 'x':
R = np.array([[1,             0,            0],

elif axis == 'y':
[            0,  1,             0 ],

elif axis == 'z':
[            0,             0,  1]])

else:
raise IOError('"axis" has to be "x", "y", or "z"')
return R

def _check_axis(sel_axis):
"""Leaves u[x/y/z] nchanged, but converts [0/1/2] to [x/y/z]

Parameters
----------
sel_axis : str or int
If "str", the value has to be 'x', 'y', or 'z'
If "int", the value has to be 0, 1, or 2

Returns
-------
axis : str
Selected axis, as string
"""

seq = 'xyz'
if type(sel_axis) is str:
if sel_axis not in seq:
raise IOError
axis = sel_axis
elif type(sel_axis) is int:
if sel_axis in range(3):
axis = seq[sel_axis]
else:
raise IOError

return axis

[docs]def R_s(axis='x', angle='alpha'):
"""
Symbolic rotation matrix about the given axis, by an angle with the given name

Parameters
----------
axis : int or str
Axis of rotation, has to be 0, 1, or 2, or 'x', 'y', or 'z'
alpha : string
name of rotation angle

Returns
-------
R_symbolic : symbolic matrix for rotation about the given axis

Examples
--------

>>> R_yaw = R_s(axis=2, angle='theta')

>>> R_nautical = R_s(2) * R_s(1) * R_s(0)

"""

axis = _check_axis(axis)

alpha = sympy.Symbol(angle)

if axis == 'x':
R_s =  sympy.Matrix([[1,                0,                 0],
[0, sympy.cos(alpha), -sympy.sin(alpha)],
[0, sympy.sin(alpha),  sympy.cos(alpha)]])

elif axis == 'y':
R_s = sympy.Matrix([[sympy.cos(alpha),0, sympy.sin(alpha)],
[0,1,0],
[-sympy.sin(alpha), 0, sympy.cos(alpha)]])

elif axis == 'z':
R_s = sympy.Matrix([[sympy.cos(alpha), -sympy.sin(alpha), 0],
[sympy.sin(alpha), sympy.cos(alpha), 0],
[0, 0, 1]])

else:
raise IOError('"axis" has to be "x", "y", or "z", not {0}'.format(axis))
return R_s

[docs]def sequence(R, to ='Euler'):
"""
This function takes a rotation matrix, and calculates
the corresponding angles for sequential rotations.

R_Euler = R3(gamma) * R1(beta) * R3(alpha)

Parameters
----------
R : ndarray, 3x3
rotation matrix
to : string
Has to be one the following:

- Euler ... Rz * Rx * Rz
- Fick ... Rz * Ry * Rx
- nautical ... same as "Fick"
- Helmholtz ... Ry * Rz * Rx

Returns
-------
gamma : third rotation (left-most matrix)
beta : second rotation
alpha : first rotation(right-most matrix)

Notes
-----
The following formulas are used:

Euler:

.. math::
\\beta   = -arcsin(\\sqrt{ R_{xz}^2 + R_{yz}^2 }) * sign(R_{yz})

\\gamma = arcsin(\\frac{R_{xz}}{sin\\beta})

\\alpha   = arcsin(\\frac{R_{zx}}{sin\\beta})

nautical / Fick:

.. math::

\\theta   = arctan(\\frac{R_{yx}}{R_{xx}})

\\phi = arcsin(R_{zx})

\\psi   = arctan(\\frac{R_{zy}}{R_{zz}})

Note that it is assumed that psi < pi !

Helmholtz:

.. math::
\\theta = arcsin(R_{yx})

\\phi = -arcsin(\\frac{R_{zx}}{cos\\theta})

\\psi = -arcsin(\\frac{R_{yz}}{cos\\theta})

Note that it is assumed that psi < pi

"""

# Reshape the input such that I can use the standard matrix indices
# For a simple (3,3) matrix, a superfluous first index is added.
Rs = R.reshape((-1,3,3), order='C')

if to=='Fick' or to=='nautical':
gamma =  np.arctan2(Rs[:,1,0], Rs[:,0,0])
alpha =  np.arctan2(Rs[:,2,1], Rs[:,2,2])
beta  = -np.arcsin(Rs[:,2,0])

elif to == 'Helmholtz':
gamma =  np.arcsin( Rs[:,1,0] )
beta  = -np.arcsin( Rs[:,2,0]/np.cos(gamma) )
alpha = -np.arcsin( Rs[:,1,2]/np.cos(gamma) )

elif to == 'Euler':
epsilon = 1e-6
beta = - np.arcsin(np.sqrt(Rs[:,0,2]**2 + Rs[:,1,2]**2)) * np.sign(Rs[:,1,2])
small_indices =  beta < epsilon

# Assign memory for alpha and gamma
alpha = np.nan * np.ones_like(beta)
gamma = np.nan * np.ones_like(beta)

# For small beta
beta[small_indices] = 0
gamma[small_indices] = 0
alpha[small_indices] = np.arcsin(Rs[small_indices,1,0])

# for the rest
gamma[~small_indices] = np.arcsin( Rs[~small_indices,0,2]/np.sin(beta) )
alpha[~small_indices] = np.arcsin( Rs[~small_indices,2,0]/np.sin(beta) )

else:
print('\nSorry, only know: \nnautical, \nFick, \nHelmholtz, \nEuler.\n')
raise IOError

# Return the parameter-angles
if R.size == 9:
return np.r_[gamma, beta, alpha]
else:
return np.column_stack( (gamma, beta, alpha) )

[docs]def dh(theta=0, d=0, r=0, alpha=0):
"""
Denavit Hartenberg transformation and rotation matrix.

.. math::
T_n^{n - 1}= {Trans}_{z_{n - 1}}(d_n)
\\cdot {Rot}_{z_{n - 1}}(\\theta_n) \\cdot {Trans}_{x_n}(r_n) \\cdot {Rot}_{x_n}(\\alpha_n)

.. math::
T_n=\\left[\\begin{array}{ccc|c}
\\cos\\theta_n & -\\sin\\theta_n \\cos\\alpha_n & \\sin\\theta_n \\sin\\alpha_n & r_n\\cos\\theta_n \\\\
\\sin\\theta_n & \\cos\\theta_n \\cos\\alpha_n & -\\cos\\theta_n \\sin\\alpha_n & r_n \\sin\\theta_n \\\\
0 & \\sin\\alpha_n & \\cos\\alpha_n & d_n \\\\
\\hline
0 & 0 & 0 & 1
\\end{array}
\\right] =\\left[\\begin{array}{ccc|c}
& & & \\\\
& R & & T \\\\
& & & \\\\
\\hline
0 & 0 & 0 & 1
\\end{array}\\right]

Examples
--------

>>> theta_1=90.0
>>> theta_2=90.0
>>> theta_3=0.
>>> dh(theta_1,60,0,0)*dh(0,88,71,90)*dh(theta_2,15,0,0)*dh(0,0,174,-180)*dh(theta_3,15,0,0)
[[-6.12323400e-17 -6.12323400e-17 -1.00000000e+00 -7.10542736e-15],
[ 6.12323400e-17  1.00000000e+00 -6.12323400e-17  7.10000000e+01],
[  1.00000000e+00 -6.12323400e-17 -6.12323400e-17  3.22000000e+02],
[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

Parameters
----------
theta : float
rotation angle z axis [deg]

d : float
transformation along the z-axis

alpha : float
rotation angle x axis [deg]

r : float
transformation along the x-axis

Returns
-------
dh : ndarray(4x4)
Denavit Hartenberg transformation matrix.

"""

# Calculate Denavit-Hartenberg transformation matrix
Rx = stm(axis=0, angle=alpha)
Tx = stm(translation=[r, 0, 0])
Rz = stm(axis=2, angle=theta)
Tz = stm(translation=[0, 0, d])

t_dh = Tz @ Rz @ Tx @ Rx

return(t_dh)

[docs]def dh_s(theta=0, d=0, r=0, alpha=0):
"""
Symbolic Denavit Hartenberg transformation and rotation matrix.

>>> dh_s('theta_1',60,0,0)*dh_s(0,88,71,90)*dh_s('theta_2',15,0,0)*dh_s(0,0,174,-180)*dh_s('theta_3',15,0,0)

Parameters
----------
theta : float
rotation angle z axis [deg]

d : float
transformation along the z-axis

alpha : float
rotation angle x axis [deg]

r : float
transformation along the x-axis

Returns
-------
R : Symbolic rotation and transformation  matrix 4x4
"""

# Force the correct input type
theta_s = str(theta)
d_s     = str(d)
r_s     = str(r)
alpha_s = str(alpha)

# Calculate Denavit-Hartenberg transformation matrix
Rx = stm_s(axis=0, angle = alpha_s)
Tx = stm_s(transl = r_s + ',0,0')
Rz = stm_s(axis=2, angle = theta_s)
Tz = stm_s(transl='0,0,' + d_s)

t_dh = Tz * Rz * Tx * Rx

return(t_dh)

[docs]def convert(rMat, to ='quat'):
"""
Converts a rotation matrix to the corresponding quaternion.
Assumes that R has the shape (3,3), or the matrix elements in columns

Parameters
----------
rMat : array, shape (3,3) or (N,9)
single rotation matrix, or matrix with rotation-matrix elements.
to : string
Currently, only 'quat' is supported

Returns
-------
outQuat : array, shape (4,) or (N,4)
corresponding quaternion vector(s)

Notes
-----

.. math::
\\vec q = 0.5*copysign\\left( {\\begin{array}{*{20}{c}}
{\\sqrt {1 + {R_{xx}} - {R_{yy}} - {R_{zz}}} ,}\\\\
{\\sqrt {1 - {R_{xx}} + {R_{yy}} - {R_{zz}}} ,}\\\\
{\\sqrt {1 - {R_{xx}} - {R_{yy}} + {R_{zz}}} ,}
\\end{array}\\begin{array}{*{20}{c}}
{{R_{zy}} - {R_{yz}}}\\\\
{{R_{xz}} - {R_{zx}}}\\\\
{{R_{yx}} - {R_{xy}}}
\\end{array}} \\right)

http://en.wikipedia.org/wiki/Quaternion

Examples
--------

>>> rotMat = array([[cos(alpha), -sin(alpha), 0],
>>>    [sin(alpha), cos(alpha), 0],
>>>    [0, 0, 1]])
>>> rotmat.convert(rotMat, 'quat')
array([[ 0.99500417,  0.        ,  0.        ,  0.09983342]])

"""

if to != 'quat':
raise IOError('Only know "quat"!')

if rMat.shape == (3,3) or rMat.shape == (9,):
rMat=np.atleast_2d(rMat.ravel()).T
else:
rMat = rMat.T
q = np.zeros((4, rMat.shape[1]))

R11 = rMat[0]
R12 = rMat[1]
R13 = rMat[2]
R21 = rMat[3]
R22 = rMat[4]
R23 = rMat[5]
R31 = rMat[6]
R32 = rMat[7]
R33 = rMat[8]

# Catch small numerical inaccuracies, but produce an error for larger problems
epsilon = 1e-10
if np.min(np.vstack((1+R11-R22-R33, 1-R11+R22-R33, 1-R11-R22+R33))) < -epsilon:
raise ValueError('Problems with defintion of rotation matrices')

q[1] = 0.5 * np.copysign(np.sqrt(np.abs(1+R11-R22-R33)), R32-R23)
q[2] = 0.5 * np.copysign(np.sqrt(np.abs(1-R11+R22-R33)), R13-R31)
q[3] = 0.5 * np.copysign(np.sqrt(np.abs(1-R11-R22+R33)), R21-R12)
q[0] = np.sqrt(1-(q[1]**2+q[2]**2+q[3]**2))

return q.T

[docs]def seq2quat(rot_angles, seq='nautical'):
"""
This function takes a angles from sequenctial rotations  and calculates
the corresponding quaternions.

Parameters
----------
rot_angles : ndarray, nx3
sequential rotation angles [deg]
seq : string
Has to be one the following:

- Euler ... Rz * Rx * Rz
- Fick ... Rz * Ry * Rx
- nautical ... same as "Fick"
- Helmholtz ... Ry * Rz * Rx

Returns
-------
quats : ndarray, nx4
corresponding quaternions

Examples
--------
>>> skin.rotmat.seq2quat([90, 23.074, -90], seq='Euler')
array([[ 0.97979575,  0.        ,  0.2000007 ,  0.        ]])

Notes
-----
The equations are longish, and can be found in 3D-Kinematics, 4.1.5 "Relation to Sequential Rotations"

"""

quats = np.nan * np.ones( (rot_angles.shape[0], 4) )

if seq =='Fick' or seq =='nautical':
theta = rot_angles[:,0]
phi = rot_angles[:,1]
psi = rot_angles[:,2]

c_th, s_th = np.cos(theta/2), np.sin(theta/2)
c_ph, s_ph = np.cos(phi/2),   np.sin(phi/2)
c_ps, s_ps = np.cos(psi/2),   np.sin(psi/2)

quats[:,0] = c_th*c_ph*c_ps + s_th*s_ph*s_ps
quats[:,1] = c_th*c_ph*s_ps - s_th*s_ph*c_ps
quats[:,2] = c_th*s_ph*c_ps + s_th*c_ph*s_ps
quats[:,3] = s_th*c_ph*c_ps - c_th*s_ph*s_ps

elif seq == 'Helmholtz':
phi = rot_angles[:,0]
theta = rot_angles[:,1]
psi = rot_angles[:,2]

c_th, s_th = np.cos(theta/2), np.sin(theta/2)
c_ph, s_ph = np.cos(phi/2),   np.sin(phi/2)
c_ps, s_ps = np.cos(psi/2),   np.sin(psi/2)

quats[:,0] = c_th*c_ph*c_ps - s_th*s_ph*s_ps
quats[:,1] = c_th*c_ph*s_ps + s_th*s_ph*c_ps
quats[:,2] = c_th*s_ph*c_ps + s_th*c_ph*s_ps
quats[:,3] = s_th*c_ph*c_ps - c_th*s_ph*s_ps

elif seq == 'Euler':
alpha = rot_angles[:,0]
beta = rot_angles[:,1]
gamma = rot_angles[:,2]

c_al, s_al = np.cos(alpha/2), np.sin(alpha/2)
c_be, s_be = np.cos(beta/2),  np.sin(beta/2)
c_ga, s_ga = np.cos(gamma/2), np.sin(gamma/2)

quats[:,0] = c_al*c_be*c_ga - s_al*c_be*s_ga
quats[:,1] = c_al*s_be*c_ga + s_al*s_be*s_ga
quats[:,2] = s_al*s_be*c_ga - c_al*s_be*s_ga
quats[:,3] = c_al*c_be*s_ga + s_al*c_be*c_ga

else:
raise ValueError('Input parameter {0} not known'.format(seq))

return quats

if __name__ == '__main__':
from pprint import pprint

STM = stm(axis=0, angle=45, translation=[1, 2, 3.3])
R_z = stm(axis=2, angle=30)
T_y = stm(translation=[0, 10., 0])

pprint(STM)
pprint(R_z)
pprint(T_y)

out_s = stm_s(axis=0, angle='0', transl='x,0,z')
pprint(out_s)
Rx = stm_s(axis=0, angle='alpha')
Tx = stm_s(transl='r,0,0')
Rz = stm_s(axis=2, angle='theta')
Tz = stm_s(transl='0,0,d')
dh_mat = Tz * Rz * Tx * Rx
pprint(Rx)
pprint(Tx)
pprint(Rz)
pprint(Tz)
pprint(dh_mat)

Rx = stm_s(axis=0, angle='0')
Tx = stm_s(transl='0,0,0')
Rz = stm_s(axis=2, angle='theta')
Tz = stm_s(transl='0,0,15')
dh2 = Tz * Rz * Tx * Rx
pprint(dh2)

pprint(dh_s('theta', 15, 0, 0))

t_dh = dh(60,15,0,0)
print(t_dh)

angles = np.r_[20, 0, 0]
quat = seq2quat(angles)
print(quat)
testmat = np.array([[np.sqrt(2)/2, -np.sqrt(2)/2, 0],
[np.sqrt(2)/2,  np.sqrt(2)/2, 0],
[0, 0, 1]])

angles = sequence(testmat, to='nautical')
print(angles)

testmat2 = np.tile(np.reshape(testmat, (1,-1)), (2,1))
angles = sequence(testmat2, to='nautical')
print(angles)

print('Done testing')
correct = np.r_[[0,0,np.pi/4]]
print(R_s(1, 'gamma'))

import quat
a = np.r_[np.cos(0.1), 0,0,np.sin(0.1)]
print('The inverse of {0} is {1}'.format(a, quat.q_inv(a)))

print(R(1, 45))
print(R('y', 45))