def angvec2r(theta, v, unit='rad'):
"""
Create an SO(3) rotation matrix from rotation angle and axis
:param theta: rotation
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param v: rotation axis, 3-vector
:type v: array_like
:return: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
``angvec2r(THETA, V)`` is an SO(3) orthonormal rotation matrix
equivalent to a rotation of ``THETA`` about the vector ``V``.
Notes:
- If ``THETA == 0`` then return identity matrix.
- If ``THETA ~= 0`` then ``V`` must have a finite length.
:seealso: :func:`~angvec2tr`, :func:`~tr2angvec`
"""
assert np.isscalar(theta) and argcheck.isvector(
v, 3), "Arguments must be theta and vector"
if np.linalg.norm(v) < 10 * _eps:
return np.eye(3)
theta = argcheck.getunit(theta, unit)
# Rodrigue's equation
sk = trn.skew(vec.unitvec(v))
R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
return R
def trexp2(S, theta=None):
"""
Exponential of so(2) or se(2) matrix
:param S: so(2), se(2) matrix or equivalent velctor
:type T: numpy.ndarray, shape=(2,2) or (3,3); array_like
:param theta: motion
:type theta: float
:return: 2x2 or 3x3 matrix exponential in SO(2) or SE(2)
:rtype: numpy.ndarray, shape=(2,2) or (3,3)
An efficient closed-form solution of the matrix exponential for arguments
that are so(2) or se(2).
For so(2) the results is an SO(2) rotation matrix:
- ``trexp2(S)`` is the matrix exponential of the so(3) element ``S`` which is a 2x2
skew-symmetric matrix.
- ``trexp2(S, THETA)`` as above but for an so(3) motion of S*THETA, where ``S`` is
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
given by ``THETA``.
- ``trexp2(W)`` is the matrix exponential of the so(2) element ``W`` expressed as
a 1-vector (array_like).
- ``trexp2(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a
unit-norm vector representing a rotation axis and a rotation magnitude
given by ``THETA``. ``W`` is expressed as a 1-vector (array_like).
For se(2) the results is an SE(2) homogeneous transformation matrix:
- ``trexp2(SIGMA)`` is the matrix exponential of the se(2) element ``SIGMA`` which is
a 3x3 augmented skew-symmetric matrix.
- ``trexp2(SIGMA, THETA)`` as above but for an se(3) motion of SIGMA*THETA, where ``SIGMA``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
- ``trexp2(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as
a 3-vector which can be considered a screw motion.
- ``trexp2(TW, THETA)`` as above but for an se(2) motion of TW*THETA, where ``TW``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
:seealso: trlog, trexp2
"""
if argcheck.ismatrix(S, (3, 3)) or argcheck.isvector(S, 3):
# se(2) case
if argcheck.ismatrix(S, (3, 3)):
# augmentented skew matrix
tw = trn.vexa(S)
else:
# 3 vector
tw = argcheck.getvector(S)
if theta is None:
(tw, theta) = vec.unittwist2(tw)
else:
assert vec.isunittwist2(
tw), 'If theta is specified S must be a unit twist'
t = tw[0:2]
w = tw[2]
R = trn._rodrigues(w, theta)
skw = trn.skew(w)
V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + (
theta - math.sin(theta)) * skw @ skw
return trn.rt2tr(R, V @ t)
elif argcheck.ismatrix(S, (2, 2)) or argcheck.isvector(S, 1):
# so(2) case
if argcheck.ismatrix(S, (2, 2)):
# skew symmetric matrix
w = trn.vex(S)
else:
# 1 vector
w = argcheck.getvector(S)
if theta is not None:
assert vec.isunitvec(
w), 'If theta is specified S must be a unit twist'
# do Rodrigues' formula for rotation
return trn._rodrigues(w, theta)
else:
raise ValueError(
" First argument must be SO(2), 1-vector, SE(2) or 3-vector")
def trlog(T, check=True):
"""
Logarithm of SO(3) or SE(3) matrix
:param T: SO(3) or SE(3) matrix
:type T: numpy.ndarray, shape=(3,3) or (4,4)
:return: logarithm
:rtype: numpy.ndarray, shape=(3,3) or (4,4)
:raises: ValueError
An efficient closed-form solution of the matrix logarithm for arguments that are SO(3) or SE(3).
- ``trlog(R)`` is the logarithm of the passed rotation matrix ``R`` which will be
3x3 skew-symmetric matrix. The equivalent vector from ``vex()`` is parallel to rotation axis
and its norm is the amount of rotation about that axis.
- ``trlog(T)`` is the logarithm of the passed homogeneous transformation matrix ``T`` which will be
4x4 augumented skew-symmetric matrix. The equivalent vector from ``vexa()`` is the twist
vector (6x1) comprising [v w].
:seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`, :func:`~spatialmath.base.transformsNd.vexa`
"""
if ishom(T, check=check):
# SE(3) matrix
if trn.iseye(T):
# is identity matrix
return np.zeros((4, 4))
else:
[R, t] = trn.tr2rt(T)
if trn.iseye(R):
# rotation matrix is identity
skw = np.zeros((3, 3))
v = t
theta = 1
else:
S = trlog(R, check=False) # recurse
w = trn.vex(S)
theta = vec.norm(w)
skw = trn.skew(w / theta)
Ginv = np.eye(3) / theta - skw / 2 + (
1 / theta - 1 / np.tan(theta / 2) / 2) * skw @ skw
v = Ginv @ t
return trn.rt2m(skw, v) * theta
elif isrot(T, check=check):
# deal with rotation matrix
R = T
if trn.iseye(R):
# matrix is identity
return np.zeros((3, 3))
elif abs(np.trace(R) + 1) < 100 * _eps:
# rotation by +/- pi, +/- 3pi etc.
diagonal = R.diagonal()
k = diagonal.argmax()
mx = diagonal[k]
I = np.eye(3)
col = R[:, k] + I[:, k]
w = col / np.sqrt(2 * (1 + mx))
theta = math.pi
return trn.skew(w * theta)
else:
# general case
theta = np.arccos((np.trace(R) - 1) / 2)
skw = (R - R.T) / 2 / np.sin(theta)
return skw * theta
else:
raise ValueError("Expect SO(3) or SE(3) matrix")