def I(self, I_new): # noqa
if ismatrix(I_new, (3, 3)):
# 3x3 matrix passed
if np.any(np.abs(I_new - I_new.T) > 1e-8):
raise ValueError('3x3 matrix is not symmetric')
elif isvector(I_new, 9):
# 3x3 matrix passed as a 1d vector
I_new = I_new.reshape(3, 3)
if np.any(np.abs(I_new - I_new.T) > 1e-8):
raise ValueError('3x3 matrix is not symmetric')
elif isvector(I_new, 6):
# 6-vector passed, moments and products of inertia,
# [Ixx Iyy Izz Ixy Iyz Ixz]
I_new = np.array([[I_new[0], I_new[3], I_new[5]],
[I_new[3], I_new[1], I_new[4]],
[I_new[5], I_new[4], I_new[2]]])
elif isvector(I_new, 3):
# 3-vector passed, moments of inertia [Ixx Iyy Izz]
I_new = np.diag(I_new)
else:
raise ValueError('invalid shape passed: must be (3,3), (6,), (3,)')
self._I = I_new
def contains(self, x, tol=50 * _eps):
"""
Test if points are on the line
:param x: 3D point
:type x: 3-element array_like, or numpy.ndarray, shape=(3,N)
:param tol: Tolerance, defaults to 50*_eps
:type tol: float, optional
:raises ValueError: Bad argument
:return: Whether point is on the line
:rtype: bool or numpy.ndarray(N) of bool
``line.contains(X)`` is true if the point ``X`` lies on the line defined by
the Plucker object self.
If ``X`` is an array with 3 rows, the test is performed on every column and
an array of booleans is returned.
"""
if base.isvector(x, 3):
x = base.getvector(x)
return np.linalg.norm(np.cross(x - self.pp, self.w)) < tol
elif base.ismatrix(x, (3, None)):
return [
np.linalg.norm(np.cross(_ - self.pp, self.w)) < tol
for _ in x.T
]
else:
raise ValueError('bad argument')
def __init__(self, value):
"""
Create a new spatial vector (abstract superclass)
:param value: Value of the
- ``SpatialVector(vec)`` is a spatial vector constructed from the 6-element array-like ``vec``
- ``SpatialVector([V1, V2, ... VN])`` is a spatial vector array with N elements, constructed from the 6-element
array-like values ``Vi``
- ``SpatialVector(A)`` is a spatial vector array with N elements, constructed from the columns of the 6xN
array ``A``.
"""
# print('spatialVec6 init')
super().__init__()
if base.isvector(value, 6):
self.data = [np.array(value)]
elif base.isvector(value, 3):
self.data = [np.r_[value, 0, 0, 0]]
elif isinstance(value, SpatialVector):
self.data = [value.A]
elif base.ismatrix(value, (6, None)):
self.data = [x for x in value.T]
elif not super().arghandler(value):
raise ValueError('bad argument to constructor')
def Exp(cls, S, check=True, so3=True):
r"""
Create an SO(3) rotation matrix from so(3)
:param S: Lie algebra so(3)
:type S: numpy ndarray
:param check: check that passed matrix is valid so(3), default True
:type check: bool
:return: SO(3) rotation
:rtype: SO3 instance
- ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra
which is a 3x3 so(3) matrix (skew symmetric)
- ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist
vector (the unique elements of the so(3) skew-symmetric matrix)
- ``SO3.Exp(T)`` is a sequence of SO(3) rotations defined by an Nx3 matrix
of twist vectors, one per row.
Note:
- if :math:`\theta \eq 0` the result in an identity matrix
- an input 3x3 matrix is ambiguous, it could be the first or third case above. In this
case the parameter `so3` is the decider.
:seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if base.ismatrix(S, (-1, 3)) and not so3:
return cls([base.trexp(s, check=check) for s in S], check=False)
else:
return cls(base.trexp(S, check=check), check=False)
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 colorconvert(image, src, dst):
flag = _convertflag(src, dst)
if isinstance(image, np.ndarray) and image.ndim == 3:
# its a color image
return cv.cvtColor(image, flag)
elif base.ismatrix(image, (None, 3)):
# not an image, see if it's Nx3
image = base.getmatrix(image, (None, 3), dtype=np.float32)
image = image.reshape((-1, 1, 3))
return cv.cvtColor(image, flag).reshape((-1, 3))
def isvalid(v, check=True):
if base.isvector(v, 3):
return True
elif base.ismatrix(v, (3, 3)):
# maybe be an se(2)
if not all(v.diagonal() == 0): # check diagonal is zero
return False
if not all(v[2, :] == 0): # check bottom row is zero
return False
if not base.isskew(v[:2, :2]):
# top left 2x2is skew symmetric
return False
return True
return False
def P3(cls, p):
"""
Create a plane object from three points
:param p: Three points in the plane
:type p: numpy.ndarray, shape=(3,3)
:return: a Plane object
:rtype: Plane
"""
p = base.ismatrix(p, (3, 3))
v1 = p[:, 0]
v2 = p[:, 1]
v3 = p[:, 2]
# compute a normal
n = np.cross(v2 - v1, v3 - v1)
return cls(n, v1)
def __init__(self, m=None, r=None, I=None):
"""
Create a new spatial inertia
:param m: mass
:type m: float
:param r: centre of mass relative to link frame
:type r: 3-element array_like
:param I: inertia about the centre of mass, axes aligned with link frame
:type I: numpy.array, shape=(6,6)
- ``SpatialInertia(m, r I)`` is a spatial inertia object for a rigid-body
with mass ``m``, centre of mass at ``r`` relative to the link frame, and an
inertia matrix ``I`` (3x3) about the centre of mass.
- ``SpatialInertia(I)`` is a spatial inertia object with a value equal
to ``I`` (6x6).
:SymPy: supported
"""
super().__init__()
if m is None and r is None and I is None:
# no arguments
I = SpatialInertia._identity()
elif m is not None and r is None and I is None and base.ismatrix(
m, (6, 6)):
I = base.getmatrix(m, (6, 6))
elif m is not None and r is not None:
r = base.getvector(r, 3)
if I is None:
I = np.zeros((3, 3))
else:
I = base.getmatrix(I, (3, 3))
C = base.skew(r)
M = np.diag((m, ) * 3) # sym friendly
I = np.block([[M, m * C.T], [m * C, I + m * C @ C.T]])
else:
raise ValueError('bad values')
self.data = [I]
def isvalid(v, check=True):
"""
Test if matrix is valid twist
:param x: array to test
:type x: numpy.ndarray
:return: true of the matrix is a 6-vector or a 4x4 se(3) element
:rtype: bool
A twist can be reprented by a 6-vector or a 4x4 skew symmetric matrix,
for example::
Twist3.isvalid([1, 2, 3, 4, 5, 6])
>>> a = base.skewa([1, 2, 3, 4, 5, 6])
>>> a
array([[ 0., -6., 5., 1.],
[ 6., 0., -4., 2.],
[-5., 4., 0., 3.],
[ 0., 0., 0., 0.]])
>>> Twist3.isvalid(a)
True
>>> b=np.random.rand(4,4)
>>> Twist3.isvalid(b)
False
"""
if base.isvector(v, 6):
return True
elif base.ismatrix(v, (4, 4)):
# maybe be an se(3)
if not all(v.diagonal() == 0): # check diagonal is zero
return False
if not all(v[3, :] == 0): # check bottom row is zero
return False
if not base.isskew(v[:3, :3]):
# top left 3x3 is skew symmetric
return False
return True
return False
def isvalid(v, check=True):
"""
Test if matrix is valid twist
:param x: array to test
:type x: numpy.ndarray
:return: true of the matrix is a 6-vector or a 4x4 se(3) element
:rtype: bool
"""
if base.isvector(v, 6):
return True
elif base.ismatrix(v, (4, 4)):
# maybe be an se(3)
if not all(v.diagonal() == 0): # check diagonal is zero
return False
if not all(v[3, :] == 0): # check bottom row is zero
return False
if not base.isskew(v[:3, :3]):
# top left 3x3 is skew symmetric
return False
return True
return False
def trinterp2(start, end, s=None):
"""
Interpolate SE(2) or SO(2) matrices
:param start: initial SE(2) or SO(2) matrix value when s=0, if None then identity is used
:type start: ndarray(3,3) or ndarray(2,2) or None
:param end: final SE(2) or SO(2) matrix, value when s=1
:type end: ndarray(3,3) or ndarray(2,2)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: interpolated SE(2) or SO(2) matrix value
:rtype: ndarray(3,3) or ndarray(2,2)
:raises ValueError: bad arguments
- ``trinterp2(None, T, S)`` is a homogeneous transform (3x3) interpolated
between identity when S=0 and T (3x3) when S=1.
- ``trinterp2(T0, T1, S)`` as above but interpolated
between T0 (3x3) when S=0 and T1 (3x3) when S=1.
- ``trinterp2(None, R, S)`` is a rotation matrix (2x2) interpolated
between identity when S=0 and R (2x2) when S=1.
- ``trinterp2(R0, R1, S)`` as above but interpolated
between R0 (2x2) when S=0 and R1 (2x2) when S=1.
.. note:: Rotation angle is linearly interpolated.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> T1 = transl2(1, 2)
>>> T2 = transl2(3, 4)
>>> trinterp2(T1, T2, 0)
>>> trinterp2(T1, T2, 1)
>>> trinterp2(T1, T2, 0.5)
>>> trinterp2(None, T2, 0)
>>> trinterp2(None, T2, 1)
>>> trinterp2(None, T2, 0.5)
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`
"""
if base.ismatrix(end, (2, 2)):
# SO(2) case
if start is None:
# TRINTERP2(T, s)
th0 = math.atan2(end[1, 0], end[0, 0])
th = s * th0
else:
# TRINTERP2(T1, start= s)
if start.shape != end.shape:
raise ValueError("start and end matrices must be same shape")
th0 = math.atan2(start[1, 0], start[0, 0])
th1 = math.atan2(end[1, 0], end[0, 0])
th = th0 * (1 - s) + s * th1
return rot2(th)
elif base.ismatrix(end, (3, 3)):
if start is None:
# TRINTERP2(T, s)
th0 = math.atan2(end[1, 0], end[0, 0])
p0 = transl2(end)
th = s * th0
pr = s * p0
else:
# TRINTERP2(T0, T1, s)
if start.shape != end.shape:
raise ValueError("both matrices must be same shape")
th0 = math.atan2(start[1, 0], start[0, 0])
th1 = math.atan2(end[1, 0], end[0, 0])
p0 = transl2(start)
p1 = transl2(end)
pr = p0 * (1 - s) + s * p1
th = th0 * (1 - s) + s * th1
return base.rt2tr(rot2(th), pr)
else:
return ValueError('Argument must be SO(2) or SE(2)')
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 transl2(x, y=None):
"""
Create SE(2) pure translation, or extract translation from SE(2) matrix
**Create a translational SE(2) matrix**
:param x: translation along X-axis
:type x: float
:param y: translation along Y-axis
:type y: float
:return: SE(2) transform matrix or the translation elements of a homogeneous transform
:rtype: ndarray(3,3)
- ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3) representing a
pure translation.
- ``T = transl2( V )`` as above but the translation is given by a 2-element
list, dict, or a numpy array, row or column vector.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> transl2(3, 4)
>>> transl2([3, 4])
>>> transl2(np.array([3, 4]))
**Extract the translational part of an SE(2) matrix**
:param x: SE(2) transform matrix
:type x: ndarray(3,3)
:return: translation elements of SE(2) matrix
:rtype: ndarray(2)
- ``t = transl2(T)`` is the translational part of the SE(3) matrix ``T`` as a
2-element NumPy array.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])
>>> transl2(T)
.. note:: This function is compatible with the MATLAB version of the Toolbox. It
is unusual/weird in doing two completely different things inside the one
function.
"""
if np.isscalar(x):
T = np.identity(3)
T[:2, 2] = [x, y]
return T
elif base.isvector(x, 2):
T = np.identity(3)
T[:2, 2] = base.getvector(x, 2)
return T
elif base.ismatrix(x, (3, 3)):
return x[:2, 2]
else:
ValueError('bad argument')