import numpy as np
import math
from collections import Iterable
[docs]def vector_norm(data, axis=None, out=None):
"""
Returns length, i.e. Euclidean norm, of ndarray along axis.
"""
data = np.array(data, dtype=np.float64, copy=True)
if out is None:
if data.ndim == 1:
return math.sqrt(np.dot(data, data))
data *= data
out = np.atleast_1d(np.sum(data, axis=axis))
np.sqrt(out, out)
return out
data *= data
np.sum(data, axis=axis, out=out)
np.sqrt(out, out)
[docs]def get_rot_mat_from_homogeneous(h_mat):
"""
Returns rotation matrix from homogeneous matrix
"""
return h_mat[:-1, :-1]
[docs]def get_pos_mat_from_homogeneous(h_mat):
"""
Returns position matrix from homogeneous matrix
"""
return h_mat[:-1,-1]
[docs]def get_pose_from_homogeneous(h_mat):
"""
Returns (7,1) pose from homogeneous matrix
"""
position = get_pos_mat_from_homogeneous(h_mat)
orientation = get_quaternion_from_matrix(
get_rot_mat_from_homogeneous(h_mat))
return np.hstack((position, orientation))
[docs]def get_rpy_from_matrix(R):
"""
Returns roll pitch, yaw from Rotation matrix
"""
r = np.arctan2(R[2, 1], R[2, 2])
p = np.arctan2(-R[2, 0], np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2))
y = np.arctan2(R[1, 0], R[0, 0])
return np.asarray([r, p, y])
[docs]def get_rpy_from_quaternion(q, convention='wxyz'):
"""
Returns roll pitch, yaw from quaternion
"""
if convention == 'xyzw':
x, y, z, w = q[0], q[1], q[2], q[3] # (N,)
elif convention == 'wxyz':
w, x, y, z = q[0], q[1], q[2], q[3] # (N,)
roll = np.arctan2(2 * (w*x + y*z), 1 - 2 * (x**2 + y**2)) # (N,)
pitch = np.arcsin(2 * (w*y - z*x)) # (N,)
yaw = np.arctan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2)) # (N,)
rpy = np.asarray([roll, pitch, yaw]).T # (N,3)
return rpy
[docs]def get_matrix_from_rpy(rpy):
"""
Returns rotation matrix from rpy
"""
cr, cp, cy = [np.cos(i) for i in rpy]
sr, sp, sy = [np.sin(i) for i in rpy]
R = np.array([[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
[sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
[-sp, cp*sr, cp*cr]])
return R
[docs]def get_matrix_from_axis_angle(axis, angle):
"""
Returns rotation matrix from axis angle
"""
x, y, z = axis
theta = angle
c, s = np.cos(theta), np.sin(theta)
v = 1 - c
R = np.array([[x ** 2 * v + c, x * y * v - z * s, x * z * v + y * s],
[x * y * v + z * s, y ** 2 * v + c, y * z * v - x * s],
[x * z * v - y * s, y * z * v + x * s, z ** 2 * v + c]])
return R
[docs]def get_matrix_from_quaternion(q, convention='wxyz'):
"""
Returns rotation matrix from quaternion
"""
if isinstance(q, Iterable):
if convention == 'xyzw':
x, y, z, w = q
elif convention == 'wxyz':
w, x, y, z = q
else:
raise TypeError
R = np.array([[2 * (w**2 + x**2) - 1, 2 * (x*y - w*z), 2 * (x*z + w*y)],
[2 * (x*y + w*z), 2 * (w**2 + y**2) - 1, 2*(y*z - w*x)],
[2 * (x*z - w*y), 2 * (y*z + w*x), 2 * (w**2 + z**2) - 1]])
return R
[docs]def get_quaternion_from_rpy(rpy, convention='wxyz'):
"""
Returns quaternion from rpy
"""
rpy = np.asarray(rpy)
multiple_rpy = True
if len(rpy.shape) < 2:
multiple_rpy = False
rpy = np.array([rpy]) # (1,3)
r, p, y = rpy[:, 0], rpy[:, 1], rpy[:, 2]
cr, sr = np.cos(r/2.), np.sin(r/2.)
cp, sp = np.cos(p/2.), np.sin(p/2.)
cy, sy = np.cos(y/2.), np.sin(y/2.)
w = cr * cp * cy + sr * sp * sy # (N,)
x = sr * cp * cy - cr * sp * sy # (N,)
y = cr * sp * cy + sr * cp * sy # (N,)
z = cr * cp * sy - sr * sp * cy # (N,)
if convention == 'xyzw':
q = np.vstack([x, y, z, w]).T
elif convention == 'wxyz':
q = np.vstack([w, x, y, z]).T
else:
raise NotImplementedError(
"Asking for a convention that has not been implemented")
if not multiple_rpy:
return q[0]
return q
[docs]def get_quaternion_from_matrix(R, convention='wxyz'):
"""
Returns quaternion from rotation matrix
"""
w = 1./2 * np.sqrt(R[0, 0] + R[1, 1] + R[2, 2] + 1)
x, y, z = 1./2 * np.array([np.sign(R[2, 1] - R[1, 2]) * np.sqrt(R[0, 0] - R[1, 1] - R[2, 2] + 1),
np.sign(R[0, 2] - R[2, 0]) *
np.sqrt(R[1, 1] - R[2, 2] - R[0, 0] + 1),
np.sign(R[1, 0] - R[0, 1]) * np.sqrt(R[2, 2] - R[0, 0] - R[1, 1] + 1)])
if convention == 'xyzw':
return np.array([x, y, z, w])
elif convention == 'wxyz':
return np.array([w, x, y, z])
else:
raise NotImplementedError(
"Asking for a convention that has not been implemented")
[docs]def get_quaternion_from_axis_angle(axis, angle, convention='wxyz'):
"""
Returns quaternion from axis angle
"""
w = np.cos(angle / 2.)
x, y , z = np.sin(angle / 2.) * axis
if convention == 'xyzw':
return np.array([x, y, z, w])
elif convention == 'wxyz':
return np.array([w, x, y, z])
else:
raise NotImplementedError("Asking for a convention that has not been implemented")
[docs]def get_quaternion_inverse(quaternion):
"""
Returns quaternion inverse
"""
q = np.array(quaternion, dtype=np.float64, copy=True)
np.negative(q[1:], q[1:])
return q / np.dot(q, q)
[docs]def get_h_mat_from_quaternion(quaternion):
"""
Returns homogeneous rotation matrix from quaternion.
"""
q = np.array(quaternion, dtype=np.float64, copy=True)
n = np.dot(q, q)
if n < _EPS:
return np.identity(4)
q *= math.sqrt(2.0 / n)
q = np.outer(q, q)
return np.array([
[1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], 0.0],
[q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], 0.0],
[q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0],
[0.0, 0.0, 0.0, 1.0]])
[docs]def quaternion_multiply(quaternion1, quaternion0):
"""
Returns multiplication of two quaternions.
"""
w0, x0, y0, z0 = quaternion0
w1, x1, y1, z1 = quaternion1
return np.array(
[
-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
-x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
],
dtype=np.float64,
)
# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0
[docs]def get_quaternion_about_axis(angle, axis):
"""
Returns quaternion for rotation about axis.
"""
q = np.array([0.0, axis[0], axis[1], axis[2]])
qlen = vector_norm(q)
if qlen > _EPS:
q *= math.sin(angle / 2.0) / qlen
q[0] = math.cos(angle / 2.0)
return q
<<<<<<< HEAD
[docs]def get_h_mat(position, orientation):
=======
[docs]def get_homogeneous_matrix(position=np.zeros(3), orientation=np.array([1.0, 0.0, 0.0, 0.0])):
>>>>>>> ea49476fc7b0492d09cb4006dd56bb6aba81ff14
"""
Returns homogeneous matrix from position and orientation
"""
position = np.asarray(position)
orientation = np.asarray(orientation)
if orientation.shape == (3,): # RPY Euler angles
R = get_matrix_from_rpy(orientation)
elif orientation.shape == (4,): # quaternion in the form [x,y,z,w]
R = get_matrix_from_quaternion(orientation)
elif orientation.shape == (3, 3): # Rotation matrix
R = orientation
H = np.vstack((np.hstack((R, position.reshape(-1, 1))),
np.array([[0, 0, 0, 1]])))
return H
[docs]def get_inverse_homogeneous(matrix):
"""
Returns homogeneous inverse
"""
R = matrix[:3, :3].T
p = -R.dot(matrix[:3, 3].reshape(-1,1))
return np.vstack((np.hstack((R,p)),
np.array([[0, 0, 0, 1]])))
[docs]def get_identity_h_mat():
"""
Returns identity matrix
"""
return np.identity(4)
[docs]def homogeneous_to_pose(matrix):
"""
Returns pose from h_mat
"""
position = matrix[:3, -1]
quaternion = get_quaternion_from_matrix(matrix[:3, :3])
return np.concatenate((position, quaternion))
[docs]def pose_to_homogeneous(pose):
"""
Returns h_mat from pose
"""
pose = np.array(pose).flatten()
position, orientation = pose[:3], pose[3:]
return get_h_mat(position=position, orientation=orientation)
[docs]def get_quaternion(orientation, convention='wxyz'):
"""
Returns quaternion from orientation
"""
if isinstance(orientation, tuple) and len(orientation) == 2:
angle, axis = orientation
if isinstance(axis, (float, int)) and isinstance(angle, np.ndarray):
angle, axis = axis, angle
return get_quaternion_from_axis_angle(axis, angle, convention)
if isinstance(orientation, (np.ndarray, list)):
orientation = np.asarray(orientation)
if orientation.shape == (3,):
return get_quaternion_from_rpy(orientation, convention)
if orientation.shape == (4,):
if convention == 'wxyz':
x, y, z, w = orientation
return np.array([w, x, y, z])
return orientation
if orientation.shape == (3, 3):
return get_quaternion_from_matrix(orientation, convention)
else:
raise ValueError("Expecting the shape of the orientation to be (3,), (3,3), or (4,), instead got: "
"{}".format(orientation.shape))
else:
raise TypeError("Expecting the given orientation to be a np.ndarray, quaternion, tuple or list, instead got: "
"{}".format(type(orientation)))
[docs]def get_rotation_matrix(orientation):
"""
Returns rotation matrix from orientation
"""
if isinstance(orientation, tuple) and len(orientation) == 2:
angle, axis = orientation
if isinstance(axis, (float, int)) and isinstance(angle, np.ndarray):
angle, axis = axis, angle
return get_matrix_from_axis_angle(axis, angle)
if isinstance(orientation, (np.ndarray, list)):
orientation = np.asarray(orientation)
if orientation.shape == (3,): # RPY Euler angles
return get_matrix_from_rpy(orientation)
if orientation.shape == (4,): # quaternion
return get_matrix_from_quaternion(orientation)
if orientation.shape == (3, 3): # rotation matrix
return orientation
else:
raise ValueError("Expecting the shape of the orientation to be (3,), (3,3), or (4,), instead got: "
"{}".format(orientation.shape))
else:
raise TypeError("Expecting the given orientation to be a np.ndarray, quaternion, tuple or list, instead got: "
"{}".format(type(orientation)))