Source code for pytransform3d.transformations._conversions

"""Conversions between transform representations."""
import math
import numpy as np
from ._utils import (check_transform, check_pq, check_screw_axis,
check_screw_parameters, check_exponential_coordinates,
check_screw_matrix, check_transform_log,
check_dual_quaternion)
from ..rotations import (
matrix_from_quaternion, quaternion_from_matrix, axis_angle_from_matrix,
matrix_from_axis_angle, cross_product_matrix, q_conj,
concatenate_quaternions, axis_angle_from_quaternion, norm_angle, eps)

[docs]def transform_from(R, p, strict_check=True):
"""Make transformation from rotation matrix and translation.

Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix

p : array-like, shape (3,)
Translation

strict_check : bool, optional (default: True)
Raise a ValueError if the transformation matrix is not numerically
close enough to a real transformation matrix. Otherwise we print a
warning.

Returns
-------
A2B : array-like, shape (4, 4)
Transform from frame A to frame B
"""
A2B = rotate_transform(
np.eye(4), R, strict_check=strict_check, check=False)
A2B = translate_transform(
A2B, p, strict_check=strict_check, check=False)
return A2B

[docs]def translate_transform(A2B, p, strict_check=True, check=True):
"""Sets the translation of a transform.

Parameters
----------
A2B : array-like, shape (4, 4)
Transform from frame A to frame B

p : array-like, shape (3,)
Translation

strict_check : bool, optional (default: True)
Raise a ValueError if the transformation matrix is not numerically
close enough to a real transformation matrix. Otherwise we print a
warning.

check : bool, optional (default: True)
Check if transformation matrix is valid

Returns
-------
A2B : array-like, shape (4, 4)
Transform from frame A to frame B
"""
if check:
A2B = check_transform(A2B, strict_check=strict_check)
out = A2B.copy()
out[:3, -1] = p
return out

[docs]def rotate_transform(A2B, R, strict_check=True, check=True):
"""Sets the rotation of a transform.

Parameters
----------
A2B : array-like, shape (4, 4)
Transform from frame A to frame B

R : array-like, shape (3, 3)
Rotation matrix

strict_check : bool, optional (default: True)
Raise a ValueError if the transformation matrix is not numerically
close enough to a real transformation matrix. Otherwise we print a
warning.

check : bool, optional (default: True)
Check if transformation matrix is valid

Returns
-------
A2B : array-like, shape (4, 4)
Transform from frame A to frame B
"""
if check:
A2B = check_transform(A2B, strict_check=strict_check)
out = A2B.copy()
out[:3, :3] = R
return out

[docs]def pq_from_transform(A2B, strict_check=True):
"""Compute position and quaternion from transformation matrix.

Parameters
----------
A2B : array-like, shape (4, 4)
Transformation matrix from frame A to frame B

strict_check : bool, optional (default: True)
Raise a ValueError if the transformation matrix is not numerically
close enough to a real transformation matrix. Otherwise we print a
warning.

Returns
-------
pq : array-like, shape (7,)
Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)
"""
A2B = check_transform(A2B, strict_check=strict_check)
return np.hstack((A2B[:3, 3], quaternion_from_matrix(A2B[:3, :3])))

[docs]def transform_from_pq(pq):
"""Compute transformation matrix from position and quaternion.

Parameters
----------
pq : array-like, shape (7,)
Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)

Returns
-------
A2B : array-like, shape (4, 4)
Transformation matrix from frame A to frame B
"""
pq = check_pq(pq)
return transform_from(matrix_from_quaternion(pq[3:]), pq[:3])

[docs]def screw_parameters_from_screw_axis(screw_axis):
"""Compute screw parameters from screw axis.

Note that there is not just one solution since q can be any point on the
screw axis. We select q so that it is orthogonal to s_axis.

Parameters
----------
screw_axis : array-like, shape (6,)
Screw axis described by 6 values
(omega_1, omega_2, omega_3, v_1, v_2, v_3),
where the first 3 components are related to rotation and the last 3
components are related to translation.

Returns
-------
q : array, shape (3,)
Vector to a point on the screw axis that is orthogonal to s_axis

s_axis : array, shape (3,)
Unit direction vector of the screw axis

h : float
Pitch of the screw. The pitch is the ratio of translation and rotation
of the screw axis. Infinite pitch indicates pure translation.
"""
screw_axis = check_screw_axis(screw_axis)

omega = screw_axis[:3]
v = screw_axis[3:]

omega_norm = np.linalg.norm(omega)
if abs(omega_norm) < np.finfo(float).eps:  # pure translation
q = np.zeros(3)
s_axis = v
h = np.inf
else:
s_axis = omega
h = omega.dot(v)
moment = v - h * s_axis
q = np.cross(s_axis, moment)
return q, s_axis, h

[docs]def screw_axis_from_screw_parameters(q, s_axis, h):
"""Compute screw axis representation from screw parameters.

Parameters
----------
q : array-like, shape (3,)
Vector to a point on the screw axis

s_axis : array-like, shape (3,)
Direction vector of the screw axis

h : float
Pitch of the screw. The pitch is the ratio of translation and rotation
of the screw axis. Infinite pitch indicates pure translation.

Returns
-------
screw_axis : array, shape (6,)
Screw axis described by 6 values
(omega_1, omega_2, omega_3, v_1, v_2, v_3),
where the first 3 components are related to rotation and the last 3
components are related to translation.
"""
q, s_axis, h = check_screw_parameters(q, s_axis, h)

if np.isinf(h):  # pure translation
return np.r_[0.0, 0.0, 0.0, s_axis]
return np.r_[s_axis, np.cross(q, s_axis) + h * s_axis]

[docs]def screw_axis_from_exponential_coordinates(Stheta):
"""Compute screw axis and theta from exponential coordinates.

Parameters
----------
Stheta : array-like, shape (6,)
Exponential coordinates of transformation:
S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
where the first 3 components are related to rotation and the last 3
components are related to translation. Theta is the rotation angle
and h * theta the translation. Theta should be >= 0. Negative rotations
will be represented by a negative screw axis instead. This is relevant
if you want to recover theta from exponential coordinates.

Returns
-------
screw_axis : array, shape (6,)
Screw axis described by 6 values
(omega_1, omega_2, omega_3, v_1, v_2, v_3),
where the first 3 components are related to rotation and the last 3
components are related to translation.

theta : float
Parameter of the transformation: theta is the angle of rotation
and h * theta the translation.
"""
Stheta = check_exponential_coordinates(Stheta)

omega_theta = Stheta[:3]
v_theta = Stheta[3:]
theta = np.linalg.norm(omega_theta)
if theta < np.finfo(float).eps:
theta = np.linalg.norm(v_theta)
if theta < np.finfo(float).eps:
return np.zeros(6), 0.0
return Stheta / theta, theta

[docs]def screw_axis_from_screw_matrix(screw_matrix):
"""Compute screw axis from screw matrix.

Parameters
----------
screw_matrix : array-like, shape (4, 4)
A screw matrix consists of a cross-product matrix that represents an
axis of rotation, a translation, and a row of zeros.

Returns
-------
screw_axis : array, shape (6,)
Screw axis described by 6 values
(omega_1, omega_2, omega_3, v_1, v_2, v_3),
where the first 3 components are related to rotation and the last 3
components are related to translation.
"""
screw_matrix = check_screw_matrix(screw_matrix)

screw_axis = np.empty(6)
screw_axis[0] = screw_matrix[2, 1]
screw_axis[1] = screw_matrix[0, 2]
screw_axis[2] = screw_matrix[1, 0]
screw_axis[3:] = screw_matrix[:3, 3]
return screw_axis

[docs]def exponential_coordinates_from_screw_axis(screw_axis, theta):
"""Compute exponential coordinates from screw axis and theta.

Parameters
----------
screw_axis : array-like, shape (6,)
Screw axis described by 6 values
(omega_1, omega_2, omega_3, v_1, v_2, v_3),
where the first 3 components are related to rotation and the last 3
components are related to translation.

theta : float
Parameter of the transformation: theta is the angle of rotation
and h * theta the translation.

Returns
-------
Stheta : array, shape (6,)
Exponential coordinates of transformation:
S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
where the first 3 components are related to rotation and the last 3
components are related to translation. Theta is the rotation angle
and h * theta the translation. Theta should be >= 0. Negative rotations
will be represented by a negative screw axis instead. This is relevant
if you want to recover theta from exponential coordinates.
"""
screw_axis = check_screw_axis(screw_axis)
return screw_axis * theta

[docs]def exponential_coordinates_from_transform_log(transform_log, check=True):
"""Compute exponential coordinates from logarithm of transformation.

Parameters
----------
transform_log : array-like, shape (4, 4)
Matrix logarithm of transformation matrix: [S] * theta.

check : bool, optional (default: True)
Check if logarithm of transformation is valid

Returns
-------
Stheta : array, shape (6,)
Exponential coordinates of transformation:
S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
where S is the screw axis, the first 3 components are related to
rotation and the last 3 components are related to translation.
Theta is the rotation angle and h * theta the translation.
"""
if check:
transform_log = check_transform_log(transform_log)

Stheta = np.empty(6)
Stheta[0] = transform_log[2, 1]
Stheta[1] = transform_log[0, 2]
Stheta[2] = transform_log[1, 0]
Stheta[3:] = transform_log[:3, 3]
return Stheta

[docs]def exponential_coordinates_from_transform(A2B, strict_check=True, check=True):
"""Compute exponential coordinates from transformation matrix.

Logarithmic map.

Parameters
----------
A2B : array-like, shape (4, 4)
Transformation matrix from frame A to frame B

strict_check : bool, optional (default: True)
Raise a ValueError if the transformation matrix is not numerically
close enough to a real transformation matrix. Otherwise we print a
warning.

check : bool, optional (default: True)
Check if transformation matrix is valid

Returns
-------
Stheta : array, shape (6,)
Exponential coordinates of transformation:
S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
where S is the screw axis, the first 3 components are related to
rotation and the last 3 components are related to translation.
Theta is the rotation angle and h * theta the translation.
"""
if check:
A2B = check_transform(A2B, strict_check=strict_check)

R = A2B[:3, :3]
p = A2B[:3, 3]

if np.linalg.norm(np.eye(3) - R) < np.finfo(float).eps:
return np.r_[0.0, 0.0, 0.0, p]

omega_theta = axis_angle_from_matrix(R, check=check)
omega_unit = omega_theta[:3]
theta = omega_theta[3]

if theta == 0:
return np.r_[0.0, 0.0, 0.0, p]

omega_unit_matrix = cross_product_matrix(omega_unit)

G_inv = (np.eye(3) / theta - 0.5 * omega_unit_matrix
+ (1.0 / theta - 0.5 / np.tan(theta / 2.0))
* np.dot(omega_unit_matrix, omega_unit_matrix))
v = G_inv.dot(p)

return np.hstack((omega_unit, v)) * theta

[docs]def screw_matrix_from_screw_axis(screw_axis):
"""Compute screw matrix from screw axis.

Parameters
----------
screw_axis : array-like, shape (6,)
Screw axis described by 6 values
(omega_1, omega_2, omega_3, v_1, v_2, v_3),
where the first 3 components are related to rotation and the last 3
components are related to translation.

Returns
-------
screw_matrix : array, shape (4, 4)
A screw matrix consists of a cross-product matrix that represents an
axis of rotation, a translation, and a row of zeros.
"""
screw_axis = check_screw_axis(screw_axis)

omega = screw_axis[:3]
v = screw_axis[3:]
screw_matrix = np.zeros((4, 4))
screw_matrix[:3, :3] = cross_product_matrix(omega)
screw_matrix[:3, 3] = v
return screw_matrix

[docs]def screw_matrix_from_transform_log(transform_log):
"""Compute screw matrix from logarithm of transformation.

Parameters
----------
transform_log : array-like, shape (4, 4)
Matrix logarithm of transformation matrix: [S] * theta.

Returns
-------
screw_matrix : array, shape (4, 4)
A screw matrix consists of a cross-product matrix that represents an
axis of rotation, a translation, and a row of zeros.
"""
transform_log = check_transform_log(transform_log)

omega = np.array([
transform_log[2, 1], transform_log[0, 2], transform_log[1, 0]])
theta = np.linalg.norm(omega)
if abs(theta) < np.finfo(float).eps:
theta = np.linalg.norm(transform_log[:3, 3])
if abs(theta) < np.finfo(float).eps:
return np.zeros((4, 4)), 0.0
return transform_log / theta, theta

[docs]def transform_log_from_exponential_coordinates(Stheta):
"""Compute matrix logarithm of transformation from exponential coordinates.

Parameters
----------
Stheta : array-like, shape (6,)
Exponential coordinates of transformation:
S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
where S is the screw axis, the first 3 components are related to
rotation and the last 3 components are related to translation.
Theta is the rotation angle and h * theta the translation.

Returns
-------
transform_log : array, shape (4, 4)
Matrix logarithm of transformation matrix: [S] * theta.
"""
check_exponential_coordinates(Stheta)

omega = Stheta[:3]
v = Stheta[3:]
transform_log = np.zeros((4, 4))
transform_log[:3, :3] = cross_product_matrix(omega)
transform_log[:3, 3] = v
return transform_log

[docs]def transform_log_from_screw_matrix(screw_matrix, theta):
"""Compute matrix logarithm of transformation from screw matrix and theta.

Parameters
----------
screw_matrix : array-like, shape (4, 4)
A screw matrix consists of a cross-product matrix that represents an
axis of rotation, a translation, and a row of zeros.

theta : float
Parameter of the transformation: theta is the angle of rotation
and h * theta the translation.

Returns
-------
transform_log : array, shape (4, 4)
Matrix logarithm of transformation matrix: [S] * theta.
"""
screw_matrix = check_screw_matrix(screw_matrix)
return screw_matrix * theta

[docs]def transform_log_from_transform(A2B, strict_check=True):
"""Compute matrix logarithm of transformation from transformation.

Parameters
----------
A2B : array, shape (4, 4)
Transform from frame A to frame B

strict_check : bool, optional (default: True)
Raise a ValueError if the transformation matrix is not numerically
close enough to a real transformation matrix. Otherwise we print a
warning.

Returns
-------
transform_log : array, shape (4, 4)
Matrix logarithm of transformation matrix: [S] * theta.
"""
A2B = check_transform(A2B, strict_check=strict_check)

R = A2B[:3, :3]
p = A2B[:3, 3]

transform_log = np.zeros((4, 4))

if np.linalg.norm(np.eye(3) - R) < np.finfo(float).eps:
transform_log[:3, 3] = p
return transform_log

omega_theta = axis_angle_from_matrix(R)
omega_unit = omega_theta[:3]
theta = omega_theta[3]

if theta == 0:
return transform_log

omega_unit_matrix = cross_product_matrix(omega_unit)

G_inv = (np.eye(3) / theta - 0.5 * omega_unit_matrix
+ (1.0 / theta - 0.5 / np.tan(theta / 2.0))
* np.dot(omega_unit_matrix, omega_unit_matrix))
v = G_inv.dot(p)

transform_log[:3, :3] = omega_unit_matrix
transform_log[:3, 3] = v
transform_log *= theta

return transform_log

[docs]def transform_from_exponential_coordinates(Stheta, check=True):
"""Compute transformation matrix from exponential coordinates.

Exponential map.

Parameters
----------
Stheta : array-like, shape (6,)
Exponential coordinates of transformation:
S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
where S is the screw axis, the first 3 components are related to
rotation and the last 3 components are related to translation.
Theta is the rotation angle and h * theta the translation.

check : bool, optional (default: True)
Check if exponential coordinates are valid

Returns
-------
A2B : array, shape (4, 4)
Transformation matrix from frame A to frame B
"""
if check:
Stheta = check_exponential_coordinates(Stheta)

omega_theta = Stheta[:3]
theta = np.linalg.norm(omega_theta)

if theta == 0.0:  # only translation
return translate_transform(np.eye(4), Stheta[3:], check=check)

screw_axis = Stheta / theta
omega_unit = screw_axis[:3]
v = screw_axis[3:]

A2B = np.eye(4)
A2B[:3, :3] = matrix_from_axis_angle(np.r_[omega_unit, theta])
omega_matrix = cross_product_matrix(omega_unit)
A2B[:3, 3] = np.dot(
np.eye(3) * theta
+ (1.0 - math.cos(theta)) * omega_matrix
+ (theta - math.sin(theta)) * np.dot(omega_matrix, omega_matrix),
v)
return A2B

[docs]def transform_from_transform_log(transform_log):
"""Compute transformation from matrix logarithm of transformation.

Parameters
----------
transform_log : array-like, shape (4, 4)
Matrix logarithm of transformation matrix: [S] * theta.

Returns
-------
A2B : array, shape (4, 4)
Transform from frame A to frame B
"""
transform_log = check_transform_log(transform_log)

omega_theta = np.array([
transform_log[2, 1], transform_log[0, 2], transform_log[1, 0]])
v = transform_log[:3, 3]
theta = np.linalg.norm(omega_theta)

if theta == 0.0:  # only translation
return translate_transform(np.eye(4), v)

omega_unit = omega_theta / theta
v = v / theta

A2B = np.eye(4)
A2B[:3, :3] = matrix_from_axis_angle(np.r_[omega_unit, theta])
omega_unit_matrix = transform_log[:3, :3] / theta
G = (np.eye(3) * theta
+ (1.0 - math.cos(theta)) * omega_unit_matrix
+ (theta - math.sin(theta)) * np.dot(omega_unit_matrix,
omega_unit_matrix))
A2B[:3, 3] = np.dot(G, v)
return A2B

[docs]def dual_quaternion_from_transform(A2B):
"""Compute dual quaternion from transformation matrix.

Parameters
----------
A2B : array-like, shape (4, 4)
Transform from frame A to frame B

Returns
-------
dq : array, shape (8,)
Unit dual quaternion to represent transform:
(pw, px, py, pz, qw, qx, qy, qz)
"""
A2B = check_transform(A2B)
real = quaternion_from_matrix(A2B[:3, :3])
dual = 0.5 * concatenate_quaternions(
np.r_[0, A2B[:3, 3]], real)
return np.hstack((real, dual))

[docs]def dual_quaternion_from_pq(pq):
"""Compute dual quaternion from position and quaternion.

Parameters
----------
pq : array-like, shape (7,)
Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)

Returns
-------
dq : array, shape (8,)
Unit dual quaternion to represent transform:
(pw, px, py, pz, qw, qx, qy, qz)
"""
pq = check_pq(pq)
real = pq[3:]
dual = 0.5 * concatenate_quaternions(
np.r_[0, pq[:3]], real)
return np.hstack((real, dual))

[docs]def dual_quaternion_from_screw_parameters(q, s_axis, h, theta):
"""Compute dual quaternion from screw parameters.

Parameters
----------
q : array-like, shape (3,)
Vector to a point on the screw axis

s_axis : array-like, shape (3,)
Direction vector of the screw axis

h : float
Pitch of the screw. The pitch is the ratio of translation and rotation
of the screw axis. Infinite pitch indicates pure translation.

theta : float
Parameter of the transformation: theta is the angle of rotation
and h * theta the translation.

Returns
-------
dq : array, shape (8,)
Unit dual quaternion to represent transform:
(pw, px, py, pz, qw, qx, qy, qz)
"""
q, s_axis, h = check_screw_parameters(q, s_axis, h)

if np.isinf(h):  # pure translation
d = theta
theta = 0
else:
d = h * theta
moment = np.cross(q, s_axis)

half_distance = 0.5 * d
sin_half_angle = np.sin(0.5 * theta)
cos_half_angle = np.cos(0.5 * theta)

real_w = cos_half_angle
real_vec = sin_half_angle * s_axis
dual_w = -half_distance * sin_half_angle
dual_vec = (sin_half_angle * moment +
half_distance * cos_half_angle * s_axis)

return np.r_[real_w, real_vec, dual_w, dual_vec]

[docs]def transform_from_dual_quaternion(dq):
"""Compute transformation matrix from dual quaternion.

Parameters
----------
dq : array-like, shape (8,)
Unit dual quaternion to represent transform:
(pw, px, py, pz, qw, qx, qy, qz)

Returns
-------
A2B : array, shape (4, 4)
Transform from frame A to frame B
"""
dq = check_dual_quaternion(dq)
real = dq[:4]
dual = dq[4:]
R = matrix_from_quaternion(real)
p = 2 * concatenate_quaternions(dual, q_conj(real))[1:]
return transform_from(R=R, p=p)

[docs]def pq_from_dual_quaternion(dq):
"""Compute position and quaternion from dual quaternion.

Parameters
----------
dq : array-like, shape (8,)
Unit dual quaternion to represent transform:
(pw, px, py, pz, qw, qx, qy, qz)

Returns
-------
pq : array, shape (7,)
Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)
"""
dq = check_dual_quaternion(dq)
real = dq[:4]
dual = dq[4:]
p = 2 * concatenate_quaternions(dual, q_conj(real))[1:]
return np.hstack((p, real))

[docs]def screw_parameters_from_dual_quaternion(dq):
"""Compute screw parameters from dual quaternion.

Parameters
----------
dq : array-like, shape (8,)
Unit dual quaternion to represent transform:
(pw, px, py, pz, qw, qx, qy, qz)

Returns
-------
q : array, shape (3,)
Vector to a point on the screw axis

s_axis : array, shape (3,)
Direction vector of the screw axis

h : float
Pitch of the screw. The pitch is the ratio of translation and rotation
of the screw axis. Infinite pitch indicates pure translation.

theta : float
Parameter of the transformation: theta is the angle of rotation
and h * theta the translation.
"""
dq = check_dual_quaternion(dq, unit=True)

real = dq[:4]
dual = dq[4:]

a = axis_angle_from_quaternion(real)
s_axis = a[:3]
theta = a[3]

translation = 2 * concatenate_quaternions(dual, q_conj(real))[1:]
if abs(theta) < np.finfo(float).eps:
# pure translation
d = np.linalg.norm(translation)
if d < np.finfo(float).eps:
s_axis = np.array([1, 0, 0])
else:
s_axis = translation / d
q = np.zeros(3)
theta = d
h = np.inf
return q, s_axis, h, theta

distance = np.dot(translation, s_axis)
moment = 0.5 * (np.cross(translation, s_axis) +
(translation - distance * s_axis)
/ np.tan(0.5 * theta))
dual = np.cross(s_axis, moment)
h = distance / theta
return dual, s_axis, h, theta

"""Compute adjoint representation of a transformation matrix.

The adjoint representation of a transformation
:math:\\left[Ad_{\\boldsymbol{T}_{BA}}\\right]
from frame A to frame B translates a twist from frame A to frame B

.. math::

\\mathcal{V}_{B}

The corresponding matrix form is

.. math::

\\left[\\mathcal{V}_{B}\\right]
= \\boldsymbol{T}_{BA} \\left[\\mathcal{V}_A\\right]
\\boldsymbol{T}_{BA}^{-1}

We can also use the adjoint representation to transform a wrench from frame
A to frame B:

.. math::

\\mathcal{F}_B

Note that not only the adjoint is transposed but also the transformation is
inverted.

Adjoint representations have the following properties:

.. math::

.. math::

Parameters
----------
A2B : array-like, shape (4, 4)
Transform from frame A to frame B

strict_check : bool, optional (default: True)
Raise a ValueError if the transformation matrix is not numerically
close enough to a real transformation matrix. Otherwise we print a
warning.

check : bool, optional (default: True)
Check if transformation matrix is valid

Returns
-------
adj_A2B : array, shape (6, 6)
"""
if check:
A2B = check_transform(A2B, strict_check)

R = A2B[:3, :3]
p = A2B[:3, 3]

[docs]def norm_exponential_coordinates(Stheta):
"""Normalize exponential coordinates of transformation.

Parameters
----------
Stheta : array-like, shape (6,)
Exponential coordinates of transformation:
S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
where the first 3 components are related to rotation and the last 3
components are related to translation. Theta is the rotation angle
and h * theta the translation. Theta should be >= 0. Negative rotations
will be represented by a negative screw axis instead. This is relevant
if you want to recover theta from exponential coordinates.

Returns
-------
Stheta : array, shape (6,)
Normalized exponential coordinates of transformation with theta in
[0, pi]. Note that in the case of pure translation no normalization
is required because the representation is unique. In the case of
rotation by pi, there is an ambiguity that will be resolved so that
the screw pitch is positive.
"""
theta = np.linalg.norm(Stheta[:3])
if theta == 0.0:
return Stheta

screw_axis = Stheta / theta
q, s_axis, h = screw_parameters_from_screw_axis(screw_axis)
if abs(theta - np.pi) < eps and h < 0:
h *= -1.0
s_axis *= -1.0
theta_normed = norm_angle(theta)
h_normalized = h * theta / theta_normed
screw_axis = screw_axis_from_screw_parameters(q, s_axis, h_normalized)

return screw_axis * theta_normed