def isvalid(v, check=True):
"""
Test if matrix is valid twist
:param x: array to test
:type x: ndarray
:return: Whether the value is a 6-vector or a valid 4x4 se(3) element
:rtype: bool
A twist can be represented by a 6-vector or a 4x4 skew symmetric matrix,
for example:
.. runblock:: pycon
>>> from spatialmath import Twist3, base
>>> import numpy as np
>>> Twist3.isvalid([1, 2, 3, 4, 5, 6])
>>> a = base.skewa([1, 2, 3, 4, 5, 6])
>>> a
>>> Twist3.isvalid(a)
>>> Twist3.isvalid(np.random.rand(4,4))
"""
if base.isvector(v, 6):
return True
elif base.ismatrix(v, (4, 4)):
# maybe be an se(3)
if not base.iszerovec(v.diagonal()): # check diagonal is zero
return False
if not base.iszerovec(v[3, :]): # check bottom row is zero
return False
if check and not base.isskew(v[:3, :3]):
# top left 3x3 is skew symmetric
return False
return True
return False
def _angle_axis_sekiguchi(T, Td):
d = base.transl(Td) - base.transl(T)
R = base.t2r(Td) @ base.t2r(T).T
l = np.r_[R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]
if base.iszerovec(l):
# diagonal matrix case
if np.trace(R) > 0:
# (1,1,1) case
a = np.zeros((3, ))
else:
# (1, -1, -1), (-1, 1, -1) or (-1, -1, 1) case
a = np.pi / 2 * (np.diag(R) + 1)
# as per Sekiguchi paper
if R[1, 0] > 0 and R[2, 1] > 0 and R[0, 2] > 0:
a = np.pi / np.sqrt(2) * np.sqrt(n.diag(R) + 1)
elif R[1, 0] > 0: # (2)
a = np.pi / np.sqrt(2) * np.sqrt(
n.diag(R) @ np.r_[1, 1, -1] + 1)
elif R[0, 2] > 0: # (3)
a = np.pi / np.sqrt(2) * np.sqrt(
n.diag(R) @ np.r_[1, -1, 1] + 1)
elif R[2, 1] > 0: # (4)
a = np.pi / np.sqrt(2) * np.sqrt(
n.diag(R) @ np.r_[-1, 1, 1] + 1)
else:
# non-diagonal matrix case
ln = base.norm(l)
a = math.atan2(ln, np.trace(R) - 1) * l / ln
return np.r_[d, a]
def isprismatic(self):
"""
Test for prismatic twist (superclass property)
:return: Whether twist is purely prismatic
:rtype: bool
Example::
>>> x = Twist3.R([1,2,3], [4,5,6])
>>> x.isprismatic
False
"""
if len(self) == 1:
return base.iszerovec(self.w)
else:
return [base.iszerovec(x.w) for x in self.data]
def unit(self):
"""
Unit twist
TW.unit() is a Twist object representing a unit aligned with the Twist
TW.
"""
if base.iszerovec(self.w):
# rotational twist
return Twist2(self.S / base.norm(S.w))
else:
# prismatic twist
return Twist2(base.unitvec(self.v), [0, 0, 0])
def isunit(q, tol=10):
"""
Test if quaternion has unit length
:param v: quaternion as a 4-vector
:type v: array_like
:param tol: tolerance in units of eps
:type tol: float
:return: whether quaternion has unit length
:rtype: bool
:seealso: unit
"""
return tr.iszerovec(q, tol=tol)
def isprismatic(self):
r"""
Test for prismatic twist (superclass property)
:return: Whether twist is purely prismatic
:rtype: bool
A prismatic twist has :math:`\vec{\omega} = 0`.
Example:
.. runblock:: pycon
>>> from spatialmath import Twist3
>>> x = Twist3.Prismatic([1,2,3])
>>> x.isprismatic
>>> x = Twist3.Revolute([1,2,3], [4,5,6])
>>> x.isprismatic
"""
if len(self) == 1:
return base.iszerovec(self.w)
else:
return [base.iszerovec(x.w) for x in self.data]
def isrevolute(self):
r"""
Test for revolute twist (superclass property)
:return: Whether twist is purely revolute
:rtype: bool
A revolute twist has :math:`\vec{v} = 0`.
Example:
.. runblock:: pycon
>>> from spatialmath import Twist3
>>> x = Twist3.Prismatic([1,2,3])
>>> x.isrevolute
>>> x = Twist3.Revolute([1,2,3], [0,0,0])
>>> x.isrevolute
"""
if len(self) == 1:
return base.iszerovec(self.v)
else:
return [base.iszerovec(x.v) for x in self.data]
def unit(self):
"""
Unitize twist (superclass property)
:return: a unit twist
:rtype: Twist3 or Twist2
``twist.unit()`` is a Twist object representing a unit aligned with the
Twist ``twist``.
"""
if base.iszerovec(self.w):
# rotational twist
return Twist3(self.S / base.norm(S.w))
else:
# prismatic twist
return Twist3(base.unitvec(self.v), [0, 0, 0])
def _angle_axis(T, Td):
d = base.transl(Td) - base.transl(T)
R = base.t2r(Td) @ base.t2r(T).T
l = np.r_[R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]
if base.iszerovec(l):
# diagonal matrix case
if np.trace(R) > 0:
# (1,1,1) case
a = np.zeros((3, ))
else:
a = np.pi / 2 * (np.diag(R) + 1)
else:
# non-diagonal matrix case
ln = base.norm(l)
a = math.atan2(ln, np.trace(R) - 1) * l / ln
return np.r_[d, a]
def rodrigues(w, theta=None):
r"""
Rodrigues' formula for rotation
:param w: rotation vector
:type w: array_like(3) or array_like(1)
:param θ: rotation angle
:type θ: float or None
:return: SO(n) matrix
:rtype: ndarray(2,2) or ndarray(3,3)
Compute Rodrigues' formula for a rotation matrix given a rotation axis
and angle.
.. math::
\mat{R} = \mat{I}_{3 \times 3} + \sin \theta \skx{\hat{\vec{v}}} + (1 - \cos \theta) \skx{\hat{\vec{v}}}^2
.. runblock:: pycon
>>> from spatialmath.base import *
>>> rodrigues([1, 0, 0], 0.3)
>>> rodrigues([0.3, 0, 0])
>>> rodrigues(0.3) # 2D version
"""
w = base.getvector(w)
if base.iszerovec(w):
# for a zero so(n) return unit matrix, theta not relevant
if len(w) == 1:
return np.eye(2)
else:
return np.eye(3)
if theta is None:
w, theta = base.unitvec_norm(w)
skw = skew(w)
return np.eye(skw.shape[0]) + math.sin(theta) * skw + (
1.0 - math.cos(theta)) * skw @ skw
def isunit(q, tol=100):
"""
Test if quaternion has unit length
:param v: quaternion
:type v: array_like(4)
:param tol: tolerance in units of eps
:type tol: float
:return: whether quaternion has unit length
:rtype: bool
.. runblock:: pycon
>>> from spatialmath.base import eye, pure, isunit
>>> q = eye()
>>> isunit(q)
>>> q = pure([1, 2, 3])
>>> isunit(q)
:seealso: unit
"""
return base.iszerovec(q, tol=tol)
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")
