def _import(self, value, check=True):
if isinstance(value, np.ndarray) and self.isvalid(value, check=check):
if value.shape == (3, 3):
# it's an se(3)
return base.vexa(value)
elif value.shape == (3, ):
# it's a twist vector
return value
raise TypeError('bad type passed')
def _import(self, value, check=True):
if isinstance(value, np.ndarray) and self.isvalid(value, check=check):
if value.shape == (4, 4):
# it's an se(3)
return base.vexa(value)
elif value.shape == (6, ):
# it's a twist vector
return value
elif base.ishom(value, check=check):
return base.trlog(value, twist=True, check=False)
raise TypeError('bad type passed')
def trexp2(S, theta=None, check=True):
"""
Exponential of so(2) or se(2) matrix
:param S: se(2), so(2) matrix or equivalent velctor
:type T: ndarray(3,3) or ndarray(2,2)
:param theta: motion
:type theta: float
:return: matrix exponential in SE(2) or SO(2)
:rtype: ndarray(3,3) or ndarray(2,2)
:raises ValueError: bad argument
An efficient closed-form solution of the matrix exponential for arguments
that are se(2) or so(2).
For se(2) the results is an SE(2) homogeneous transformation matrix:
- ``trexp2(Σ)`` is the matrix exponential of the se(2) element ``Σ`` which is
a 3x3 augmented skew-symmetric matrix.
- ``trexp2(Σ, θ)`` as above but for an se(3) motion of Σθ, where ``Σ``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
- ``trexp2(S)`` is the matrix exponential of the se(3) element ``S`` represented as
a 3-vector which can be considered a screw motion.
- ``trexp2(S, θ)`` as above but for an se(2) motion of Sθ, where ``S``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trexp2(skew(1))
>>> trexp2(skew(1), 2) # revolute unit twist
>>> trexp2(1)
>>> trexp2(1, 2) # revolute unit twist
For so(2) the results is an SO(2) rotation matrix:
- ``trexp2(Ω)`` is the matrix exponential of the so(3) element ``Ω`` which is a 2x2
skew-symmetric matrix.
- ``trexp2(Ω, θ)`` as above but for an so(3) motion of Ωθ, where ``Ω`` is
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
given by ``θ``.
- ``trexp2(ω)`` is the matrix exponential of the so(2) element ``ω`` expressed as
a 1-vector.
- ``trexp2(ω, θ)`` as above but for an so(3) motion of ωθ where ``ω`` is a
unit-norm vector representing a rotation axis and a rotation magnitude
given by ``θ``. ``ω`` is expressed as a 1-vector.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trexp2(skewa([1, 2, 3]))
>>> trexp2(skewa([1, 0, 0]), 2) # prismatic unit twist
>>> trexp2([1, 2, 3])
>>> trexp2([1, 0, 0], 2)
:seealso: trlog, trexp2
"""
if base.ismatrix(S, (3, 3)) or base.isvector(S, 3):
# se(2) case
if base.ismatrix(S, (3, 3)):
# augmentented skew matrix
if check and not base.isskewa(S):
raise ValueError("argument must be a valid se(2) element")
tw = base.vexa(S)
else:
# 3 vector
tw = base.getvector(S)
if base.iszerovec(tw):
return np.eye(3)
if theta is None:
(tw, theta) = base.unittwist2_norm(tw)
elif not base.isunittwist2(tw):
raise ValueError("If theta is specified S must be a unit twist")
t = tw[0:2]
w = tw[2]
R = base.rodrigues(w, theta)
skw = base.skew(w)
V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + (
theta - math.sin(theta)) * skw @ skw
return base.rt2tr(R, V @ t)
elif base.ismatrix(S, (2, 2)) or base.isvector(S, 1):
# so(2) case
if base.ismatrix(S, (2, 2)):
# skew symmetric matrix
if check and not base.isskew(S):
raise ValueError("argument must be a valid so(2) element")
w = base.vex(S)
else:
# 1 vector
w = base.getvector(S)
if theta is not None and not base.isunitvec(w):
raise ValueError("If theta is specified S must be a unit twist")
# do Rodrigues' formula for rotation
return base.rodrigues(w, theta)
else:
raise ValueError(
" First argument must be SO(2), 1-vector, SE(2) or 3-vector")
def trlog2(T, check=True, twist=False):
"""
Logarithm of SO(2) or SE(2) matrix
:param T: SE(2) or SO(2) matrix
:type T: ndarray(3,3) or ndarray(2,2)
:param check: check that matrix is valid
:type check: bool
:param twist: return a twist vector instead of matrix [default]
:type twist: bool
:return: logarithm
:rtype: ndarray(3,3) or ndarray(3); or ndarray(2,2) or ndarray(1)
:raises ValueError: bad argument
An efficient closed-form solution of the matrix logarithm for arguments that
are SO(2) or SE(2).
- ``trlog2(R)`` is the logarithm of the passed rotation matrix ``R`` which
will be 2x2 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 3x3 augumented skew-symmetric matrix. The
equivalent vector from ``vexa()`` is the twist vector (6x1) comprising [v
w].
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trlog2(trot2(0.3))
>>> trlog2(trot2(0.3), twist=True)
>>> trlog2(rot2(0.3))
>>> trlog2(rot2(0.3), twist=True)
:seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`,
:func:`~spatialmath.base.transformsNd.vexa`
"""
if ishom2(T, check=check):
# SE(2) matrix
if base.iseye(T):
# is identity matrix
if twist:
return np.zeros((3, ))
else:
return np.zeros((3, 3))
else:
if twist:
return base.vexa(scipy.linalg.logm(T))
else:
return scipy.linalg.logm(T)
elif isrot2(T, check=check):
# SO(2) rotation matrix
if twist:
return base.vex(scipy.linalg.logm(T))
else:
return scipy.linalg.logm(T)
else:
raise ValueError("Expect SO(2) or SE(2) matrix")
