#!/usr/bin/env python
"""
Homogeneous Transformation Matrices
"""
import math
import numpy as np
# Local modules
import baldor as br
[docs]def are_equal(T1, T2, rtol=1e-5, atol=1e-8):
"""
Returns True if two homogeneous transformation are equal within a tolerance.
Parameters
----------
T1: array_like
First input homogeneous transformation
T2: array_like
Second input homogeneous transformation
rtol: float
The relative tolerance parameter.
atol: float
The absolute tolerance parameter.
Returns
-------
equal : bool
True if `T1` and `T2` are `almost` equal, False otherwise
See Also
--------
numpy.allclose: Contains the details about the tolerance parameters
"""
M1 = np.array(T1, dtype=np.float64, copy=True)
M1 /= M1[3, 3]
M2 = np.array(T2, dtype=np.float64, copy=True)
M2 /= M2[3, 3]
return np.allclose(M1, M2, rtol, atol)
[docs]def between_axes(axis_a, axis_b):
"""
Compute the transformation that aligns two vectors/axes.
Parameters
----------
axis_a: array_like
The initial axis
axis_b: array_like
The goal axis
Returns
-------
transform: array_like
The transformation that transforms `axis_a` into `axis_b`
"""
a_unit = br.vector.unit(axis_a)
b_unit = br.vector.unit(axis_b)
c = np.dot(a_unit, b_unit)
angle = np.arccos(c)
if np.isclose(c, -1.0) or np.allclose(a_unit, b_unit):
axis = br.vector.perpendicular(b_unit)
else:
axis = br.vector.unit(np.cross(a_unit, b_unit))
transform = br.axis_angle.to_transform(axis, angle)
return transform
[docs]def inverse(transform):
"""
Compute the inverse of an homogeneous transformation.
.. note:: This function is more efficient than :obj:`numpy.linalg.inv` given
the special properties of homogeneous transformations.
Parameters
----------
transform: array_like
The input homogeneous transformation
Returns
-------
inv: array_like
The inverse of the input homogeneous transformation
"""
R = transform[:3, :3].T
p = transform[:3, 3]
inv = np.eye(4)
inv[:3, :3] = R
inv[:3, 3] = np.dot(-R, p)
return inv
[docs]def random(max_position=1.):
"""
Generate a random homogeneous transformation.
Parameters
----------
max_position: float, optional
Maximum value for the position components of the transformation
Returns
-------
T: array_like
The random homogeneous transformation
Examples
--------
>>> import numpy as np
>>> import baldor as br
>>> T = br.transform.random()
>>> Tinv = br.transform.inverse(T)
>>> np.allclose(np.dot(T, Tinv), np.eye(4))
True
"""
quat = br.quaternion.random()
T = br.quaternion.to_transform(quat)
T[:3, 3] = np.random.rand(3)*max_position
return T
[docs]def to_axis_angle(transform):
"""
Return rotation angle and axis from rotation matrix.
Parameters
----------
transform: array_like
The input homogeneous transformation
Returns
-------
axis: array_like
axis around which rotation occurs
angle: float
angle of rotation
point: array_like
point around which the rotation is performed
Examples
--------
>>> import numpy as np
>>> import baldor as br
>>> axis = np.random.sample(3) - 0.5
>>> angle = (np.random.sample(1) - 0.5) * (2*np.pi)
>>> point = np.random.sample(3) - 0.5
>>> T0 = br.axis_angle.to_transform(axis, angle, point)
>>> axis, angle, point = br.transform.to_axis_angle(T0)
>>> T1 = br.axis_angle.to_transform(axis, angle, point)
>>> br.transform.are_equal(T0, T1)
True
"""
R = np.array(transform, dtype=np.float64, copy=False)
R33 = R[:3, :3]
# direction: unit eigenvector of R33 corresponding to eigenvalue of 1
w, W = np.linalg.eig(R33.T)
i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
if not len(i):
raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
axis = np.real(W[:, i[-1]]).squeeze()
# point: unit eigenvector of R corresponding to eigenvalue of 1
w, Q = np.linalg.eig(R)
i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
if not len(i):
raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
point = np.real(Q[:, i[-1]]).squeeze()
point /= point[3]
# rotation angle depending on axis
cosa = (np.trace(R33) - 1.0) / 2.0
if abs(axis[2]) > 1e-8:
sina = (R[1, 0] + (cosa-1.0)*axis[0]*axis[1]) / axis[2]
elif abs(axis[1]) > 1e-8:
sina = (R[0, 2] + (cosa-1.0)*axis[0]*axis[2]) / axis[1]
else:
sina = (R[2, 1] + (cosa-1.0)*axis[1]*axis[2]) / axis[0]
angle = math.atan2(sina, cosa)
return axis, angle, point
[docs]def to_dual_quaternion(transform):
"""
Return quaternion from the rotation part of an homogeneous transformation.
Parameters
----------
transform: array_like
Rotation matrix. It can be (3x3) or (4x4)
isprecise: bool
If True, the input transform is assumed to be a precise rotation matrix and
a faster algorithm is used.
Returns
-------
qr: array_like
Quaternion in w, x, y z (real, then vector) for the rotation component
qt: array_like
Quaternion in w, x, y z (real, then vector) for the translation component
Notes
-----
Some literature prefers to use :math:`q` for the rotation component and
:math:`q'` for the translation component
"""
def cot(x): return 1./np.tan(x)
R = np.eye(4)
R[:3, :3] = transform[:3, :3]
l, theta, _ = to_axis_angle(R)
t = transform[:3, 3]
# Pitch d
d = np.dot(l.reshape(1, 3), t.reshape(3, 1))
# Point c
c = 0.5*(t-d*l) + cot(theta/2.)*np.cross(l, t)
# Moment vector
m = np.cross(c, l)
# Rotation quaternion
qr = np.zeros(4)
qr[0] = np.cos(theta/2.)
qr[1:] = np.sin(theta/2.)*l
# Translation quaternion
qt = np.zeros(4)
qt[0] = -(1/2.)*np.dot(qr[1:], t)
qt[1:] = (1/2.)*(qr[0]*t + np.cross(t, qr[1:]))
return qr, qt
[docs]def to_euler(transform, axes='sxyz'):
"""
Return Euler angles from transformation matrix with the specified axis
sequence.
Parameters
----------
transform: array_like
Rotation matrix. It can be (3x3) or (4x4)
axes: str, optional
Axis specification; one of 24 axis sequences as string or encoded tuple
Returns
-------
ai: float
First rotation angle (according to axes).
aj: float
Second rotation angle (according to axes).
ak: float
Third rotation angle (according to axes).
Notes
-----
Many Euler angle triplets can describe the same rotation matrix
Examples
--------
>>> import numpy as np
>>> import baldor as br
>>> T0 = br.euler.to_transform(1, 2, 3, 'syxz')
>>> al, be, ga = br.transform.to_euler(T0, 'syxz')
>>> T1 = br.euler.to_transform(al, be, ga, 'syxz')
>>> np.allclose(T0, T1)
True
"""
try:
firstaxis, parity, repetition, frame = br._AXES2TUPLE[axes.lower()]
except (AttributeError, KeyError):
br._TUPLE2AXES[axes] # validation
firstaxis, parity, repetition, frame = axes
i = firstaxis
j = br._NEXT_AXIS[i+parity]
k = br._NEXT_AXIS[i-parity+1]
M = np.array(transform, dtype=np.float64, copy=False)[:3, :3]
if repetition:
sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
if sy > br._EPS:
ax = math.atan2(M[i, j], M[i, k])
ay = math.atan2(sy, M[i, i])
az = math.atan2(M[j, i], -M[k, i])
else:
ax = math.atan2(-M[j, k], M[j, j])
ay = math.atan2(sy, M[i, i])
az = 0.0
else:
cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
if cy > br._EPS:
ax = math.atan2(M[k, j], M[k, k])
ay = math.atan2(-M[k, i], cy)
az = math.atan2(M[j, i], M[i, i])
else:
ax = math.atan2(-M[j, k], M[j, j])
ay = math.atan2(-M[k, i], cy)
az = 0.0
if parity:
ax, ay, az = -ax, -ay, -az
if frame:
ax, az = az, ax
return ax, ay, az
[docs]def to_quaternion(transform, isprecise=False):
"""
Return quaternion from the rotation part of an homogeneous transformation.
Parameters
----------
transform: array_like
Rotation matrix. It can be (3x3) or (4x4)
isprecise: bool
If True, the input transform is assumed to be a precise rotation matrix and
a faster algorithm is used.
Returns
-------
q: array_like
Quaternion in w, x, y z (real, then vector) format
Notes
-----
Quaternions :math:`w + ix + jy + kz` are represented as :math:`[w, x, y, z]`.
Examples
--------
>>> import numpy as np
>>> import baldor as br
>>> q = br.transform.to_quaternion(np.identity(4), isprecise=True)
>>> np.allclose(q, [1, 0, 0, 0])
True
>>> q = br.transform.to_quaternion(np.diag([1, -1, -1, 1]))
>>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])
True
>>> T = br.axis_angle.to_transform((1, 2, 3), 0.123)
>>> q = br.transform.to_quaternion(T, True)
>>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
True
"""
M = np.array(transform, dtype=np.float64, copy=False)[:4, :4]
if isprecise:
q = np.empty((4, ))
t = np.trace(M)
if t > M[3, 3]:
q[0] = t
q[3] = M[1, 0] - M[0, 1]
q[2] = M[0, 2] - M[2, 0]
q[1] = M[2, 1] - M[1, 2]
else:
i, j, k = 0, 1, 2
if M[1, 1] > M[0, 0]:
i, j, k = 1, 2, 0
if M[2, 2] > M[i, i]:
i, j, k = 2, 0, 1
t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
q[i] = t
q[j] = M[i, j] + M[j, i]
q[k] = M[k, i] + M[i, k]
q[3] = M[k, j] - M[j, k]
q = q[[3, 0, 1, 2]]
q *= 0.5 / math.sqrt(t * M[3, 3])
else:
m00 = M[0, 0]
m01 = M[0, 1]
m02 = M[0, 2]
m10 = M[1, 0]
m11 = M[1, 1]
m12 = M[1, 2]
m20 = M[2, 0]
m21 = M[2, 1]
m22 = M[2, 2]
# symmetric matrix K
K = np.array([[m00-m11-m22, 0.0, 0.0, 0.0],
[m01+m10, m11-m00-m22, 0.0, 0.0],
[m02+m20, m12+m21, m22-m00-m11, 0.0],
[m21-m12, m02-m20, m10-m01, m00+m11+m22]])
K /= 3.0
# quaternion is eigenvector of K that corresponds to largest eigenvalue
w, V = np.linalg.eigh(K)
q = V[[3, 0, 1, 2], np.argmax(w)]
if q[0] < 0.0:
np.negative(q, q)
return q