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 __init__(self, arg=None, w=None, check=True):
"""
Construct a new 2D Twist object
:type a: 2-element array-like
:return: 2D prismatic twist
:rtype: Twist2 instance
- ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as
a 2x2 matrix.
- ``Twist2(T)`` is a 2D Twist object representing the SE(2) rigid-body motion expressed as
a 3x3 matrix.
- ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation,
and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion
- ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the
moment vector (1 element) and direction vector (2 elements).
"""
if w is None:
# zero or one arguments passed
if super().arghandler(arg, convertfrom=(SE2, ), check=check):
return
elif base.isvector(arg, 6):
# Twist(array_like)
self.data = [base.getvector(arg)]
return
elif w is not None and base.isvector(w, 1) and base.isvector(arg, 2):
# Twist(v, w)
self.data = [np.r_[arg, w]]
return
raise ValueError('bad twist value')
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 __init__(self, arg=None, w=None, check=True):
"""
Construct a new 3D twist object
- ``Twist3()`` is a Twist3 instance representing null motion -- the
identity twist
- ``Twist3(S)`` is a Twist3 instance from an array-like (6,)
- ``Twist3(v, w)`` is a Twist3 instance from a moment ``v`` (3,) and
direction ``w`` (3,)
- ``Twist3([S1, S2, ... SN])`` where each ``Si`` is a numpy array (6,)
- ``Twist3(X)`` is a Twist3 instance with the same value as ``X``, ie.
a copy
- ``Twist3([X1, X2, ... XN])`` where each Xi is a Twist3 instance, is a
Twist3 instance containing N motions
"""
super().__init__()
if w is None:
# zero or one arguments passed
if super().arghandler(arg, check=check):
return
elif isinstance(arg, SE3):
self.data = [arg.twist().A]
elif w is not None and base.isvector(w, 3) and base.isvector(arg, 3):
# Twist(v, w)
self.data = [np.r_[arg, w]]
return
else:
raise ValueError('bad value to Twist constructor')
def __init__(self, arg=None, w=None, check=True):
"""
Construct a new 3D twist object
- ``Twist3()`` is a Twist3 instance representing null motion -- the
identity twist
- ``Twist3(t)`` is a Twist3 instance from an array-like (6,)
- ``Twist3(v, w)`` is a Twist3 instance from a moment ``v`` (3,) and
direction ``w`` (3,)
- ``Twist3([t1, t2, ... tN])`` where each ti is a numpy array (6,)
- ``Twist3([X1, X2, ... XN])`` where each Xi is a Twist3 instance, is a
Twist3 instance containing N motions
"""
super().__init__()
if w is None:
# zero or one arguments passed
if super().arghandler(arg, convertfrom=(SE3, ), check=check):
return
elif w is not None and base.isvector(w, 3) and base.isvector(arg, 3):
# Twist(v, w)
self.data = [np.r_[arg, w]]
return
raise ValueError('bad twist value')
def __mul__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Product of dual quaternion
- ``dq1 * dq2`` is a dual quaternion representing the product of
``dq1`` and ``dq2``. If both are unit dual quaternions, the product
will be a unit dual quaternion.
- ``dq * p`` transforms the point ``p`` (3) by the unit dual quaternion
``dq``.
Example:
.. runblock:: pycon
>>> from spatialmath import DualQuaternion, Quaternion
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
>>> d * d
"""
if isinstance(right, DualQuaternion):
real = left.real * right.real
dual = left.real * right.dual + left.dual * right.real
if isinstance(left, UnitDualQuaternion) and isinstance(left, UnitDualQuaternion):
return UnitDualQuaternion(real, dual)
else:
return DualQuaternion(real, dual)
elif isinstance(left, UnitDualQuaternion) and base.isvector(right, 3):
v = base.getvector(right, 3)
vp = left * DualQuaternion.Pure(v) * left.conj()
return vp.dual.v
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 e2h(v):
"""
Convert from Euclidean to homogeneous form
:param v: Euclidean vector or matrix
:type v: array_like(n), ndarray(n,m)
:return: homogeneous vector
:rtype: ndarray(n+1,m)
- If ``v`` is an N-vector, return an (N+1)-column vector where a value of 1 has
been appended as the last element.
- If ``v`` is a matrix (NxM), return a matrix (N+1xM), where each column has
been appended with a value of 1, ie. a row of ones has been appended to the matrix.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> e2h([2, 4, 6])
>>> e = np.c_[[1,2], [3,4], [5,6]]
>>> e
>>> e2h(e)
.. note:: The result is always a 2D array, a 1D input results in a column vector.
:seealso: e2h
"""
if isinstance(v, np.ndarray) and len(v.shape) == 2:
# dealing with matrix
return np.vstack([v, np.ones((1, v.shape[1]))])
elif base.isvector(v):
# dealing with shape (N,) array
v = base.getvector(v, out='col')
return np.vstack((v, 1))
def Eul(cls, angles, *, unit='rad'):
r"""
Create an SE(3) pure rotation from Euler angles
:param 𝚪: Euler angles
:type 𝚪: array_like or numpy.ndarray with shape=(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: SE(3) matrix
:rtype: SE3 instance
``SE3.Eul(𝚪)`` is an SE(3) rotation defined by a 3-vector of Euler
angles :math:`\Gamma=(\phi, \theta, \psi)` which correspond to consecutive
rotations about the Z, Y, Z axes respectively.
If ``𝚪`` is an Nx3 matrix then the result is a sequence of
rotations each defined by Euler angles corresponding to the rows of
``𝚪``.
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.base.transforms3d.eul2r`
:SymPy: supported
"""
if base.isvector(angles, 3):
return cls(base.eul2tr(angles, unit=unit), check=False)
else:
return cls([base.eul2tr(a, unit=unit) for a in angles],
check=False)
def EulerVec(cls, w):
r"""
Construct a new SO(3) rotation matrix from an Euler rotation vector
:param ω: rotation axis
:type ω: 3-element array_like
:return: SO(3) rotation
:rtype: SO3 instance
``SO3.EulerVec(ω)`` is a unit quaternion that describes the 3D rotation
defined by a rotation of :math:`\theta = \lVert \omega \rVert` about the
unit 3-vector :math:`\omega / \lVert \omega \rVert`.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> SO3.EulerVec([0.5,0,0])
.. note:: :math:`\theta \eq 0` the result in an identity matrix, otherwise
``V`` must have a finite length, ie. :math:`|V| > 0`.
:seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`~spatialmath.base.transforms3d.angvec2r`
"""
assert base.isvector(w, 3), 'w must be a 3-vector'
w = base.getvector(w)
theta = base.norm(w)
return cls(base.angvec2r(theta, w), 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 h2e(v):
"""
Convert from homogeneous to Euclidean form
:param v: homogeneous vector or matrix
:type v: array_like(n), ndarray(n,m)
:return: Euclidean vector
:rtype: ndarray(n-1), ndarray(n-1,m)
- If ``v`` is an N-vector, return an (N-1)-column vector where the elements have
all been scaled by the last element of ``v``.
- If ``v`` is a matrix (NxM), return a matrix (N-1xM), where each column has
been scaled by its last element.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> h2e([2, 4, 6, 1])
>>> h2e([2, 4, 6, 2])
>>> h = np.c_[[1,2,1], [3,4,2], [5,6,1]]
>>> h
>>> h2e(h)
.. note:: The result is always a 2D array, a 1D input results in a column vector.
:seealso: e2h
"""
if isinstance(v, np.ndarray) and len(v.shape) == 2:
# dealing with matrix
return v[:-1, :] / np.tile(v[-1, :], (v.shape[0] - 1, 1))
elif base.isvector(v):
# dealing with shape (N,) array
v = base.getvector(v, out='col')
return v[0:-1] / v[-1]
def __init__(self, v=None, w=None):
"""
Create a Plucker 3D line object
:param v: Plucker vector, Plucker object, Plucker moment
:type v: 6-element array_like, Plucker instance, 3-element array_like
:param w: Plucker direction, optional
:type w: 3-element array_like, optional
:raises ValueError: bad arguments
:return: Plucker line
:rtype: Plucker
- ``L = Plucker(X)`` creates a Plucker object from the Plucker coordinate vector
``X`` = [V,W] where V (3-vector) is the moment and W (3-vector) is the line direction.
- ``L = Plucker(L)`` creates a copy of the Plucker object ``L``.
- ``L = Plucker(V, W)`` creates a Plucker object from moment ``V`` (3-vector) and
line direction ``W`` (3-vector).
Notes:
- The Plucker object inherits from ``collections.UserList`` and has list-like
behaviours.
- A single Plucker object contains a 1D array of Plucker coordinates.
- The elements of the array are guaranteed to be Plucker coordinates.
- The number of elements is given by ``len(L)``
- The elements can be accessed using index and slice notation, eg. ``L[1]`` or
``L[2:3]``
- The Plucker instance can be used as an iterator in a for loop or list comprehension.
- Some methods support operations on the internal list.
:seealso: Plucker.PQ, Plucker.Planes, Plucker.PointDir
"""
super().__init__() # enable list powers
if w is None:
# zero or one arguments passed
if super().arghandler(v, convertfrom=(SE3, )):
return
else:
# additional arguments
assert base.isvector(v, 3) and base.isvector(
w, 3), 'expecting two 3-vectors'
self.data = [np.r_[v, w]]
def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:rtype: SE3 instance
There are multiple call signatures that return an ``SE3`` instance
with one or more values.
- ``SE3()`` null motion, value is the identity matrix.
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` where ``T`` is a 4x4 Numpy array representing an SE(3)
matrix. If ``check`` is ``True`` check the matrix belongs to SE(3).
- ``SE3([T1, T2, ... TN])`` has ``N`` values
given by the elements ``Ti`` each of which is a 4x4 NumPy array
representing an SE(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SE(3).
- ``SE3(X)`` where ``X`` is:
- ``SE3`` is a copy of ``X``
- ``SO3`` is the rotation of ``X`` with zero translation
- ``SE2`` is the z-axis rotation and x- and y-axis translation of
``X``
- ``SE3([X1, X2, ... XN])`` has ``N`` values
given by the elements ``Xi`` each of which is an SE3 instance.
:SymPy: supported
"""
if y is None and z is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif isinstance(x, SO3):
self.data = [base.r2t(_x) for _x in x.data]
elif type(x).__name__ == 'SE2':
def convert(x):
# convert SE(2) to SE(3)
out = np.identity(4, dtype=x.dtype)
out[:2, :2] = x[:2, :2]
out[:2, 3] = x[:2, 2]
return out
self.data = [convert(_x) for _x in x.data]
elif base.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [base.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [base.transl(T) for T in x]
else:
raise ValueError('bad argument to constructor')
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [base.transl(x, y, z)]
def RPY(cls, *angles, unit='rad', order='zyx'):
r"""
Create an SE(3) pure rotation from roll-pitch-yaw angles
:param 𝚪: roll-pitch-yaw angles
:type 𝚪: array_like or numpy.ndarray with shape=(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type order: str
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.RPY(𝚪)`` is an SE(3) rotation defined by a 3-vector of roll,
pitch, yaw angles :math:`\Gamma=(r, p, y)` which correspond to
successive rotations about the axes specified by ``order``:
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. This is the **convention** for a mobile robot with x-axis forward
and y-axis sideways.
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. This is the **convention** for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll about the new z-axis. This is the **convention** for a camera with z-axis parallel
to the optical axis and x-axis parallel to the pixel rows.
If ``𝚪`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
corresponding to the rows of ``𝚪``.
- ``SE3.RPY(⍺, β, 𝛾)`` as above but the angles are provided as three
scalars.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.RPY(0.1, 0.2, 0.3)
>>> SE3.RPY([0.1, 0.2, 0.3])
>>> SE3.RPY(0.1, 0.2, 0.3, order='xyz')
>>> SE3.RPY(10, 20, 30, unit='deg')
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.base.transforms3d.rpy2r`
:SymPy: supported
"""
if len(angles) == 1:
angles = angles[0]
if base.isvector(angles, 3):
return cls(base.rpy2tr(angles, order=order, unit=unit),
check=False)
else:
return cls(
[base.rpy2tr(a, order=order, unit=unit) for a in angles],
check=False)
def RPY(cls, *angles, unit='rad', order='zyx', ):
r"""
Construct a new SO(3) from roll-pitch-yaw angles
:param angles: roll-pitch-yaw angles
:type angles: array_like(3), array_like(n,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type order: str
:return: SO(3) rotation
:rtype: SO3 instance
- ``SO3.RPY(angles)`` is an SO(3) rotation defined by a 3-vector of
roll, pitch, yaw angles :math:`(\alpha, \beta, \gamma)`. If ``angles``
is an Nx3 matrix then the result is a sequence of rotations each
defined by RPY angles corresponding to the rows of angles. The angles
correspond to successive rotations about the axes specified by
``order``:
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
and y-axis sideways.
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll about the new z-axis. Convention for a camera with z-axis parallel
to the optic axis and x-axis parallel to the pixel rows.
- ``SO3.RPY(⍺, β, 𝛾)`` as above but the angles are provided as three
scalars.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> SO3.RPY(0.1, 0.2, 0.3)
>>> SO3.RPY([0.1, 0.2, 0.3])
>>> SO3.RPY(0.1, 0.2, 0.3, order='xyz')
>>> SO3.RPY(10, 20, 30, 'deg')
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r`
"""
if len(angles) == 1:
angles = angles[0]
# angles = base.getmatrix(angles, (None, 3))
# return cls(base.rpy2r(angles, order=order, unit=unit), check=False)
if base.isvector(angles, 3):
return cls(base.rpy2r(angles, unit=unit, order=order), check=False)
else:
return cls([base.rpy2r(a, unit=unit, order=order) for a in angles], check=False)
def tristim2cc(tri):
"""
Tristimulus to chromaticity coordinates
:param tri: xyz as an array, (N,3) or (N,M,3)
:type tri: float or array_like
:return cc: chromaticity coordinates
:rtype: numpy array, shape = (N,2) or (N,M,2)
``tristim2cc(tri)`` is the chromaticity coordinate (1x2) corresponding
to the tristimulus ``tri`` (1,3). Multiple tristimulus values can be
given as rows of ``tri`` (N,3), in which case the chromaticity
coordinates are the corresponding rows (N,2).
``tristim2cc(im)`` is the chromaticity coordinates corresponding to every
pixel in the tristimulus image ``im`` (H,W,3). The return is (H,W,2) that
has planes corresponding to r and g, or x and y (depending on whether the
input image was rgb or xyz).
Example:
.. runblock:: pycon
:references:
- Robotics, Vision & Control, Chapter 10, P. Corke, Springer 2011.
"""
# TODO check if tri is correct shape? can be vector or matrix
tri = np.array(tri)
if tri.ndim == 2 and tri.shape[-1] == 3:
# N x 3 case
# each row is R G B, or X Y Z
s = np.sum(tri, axis=1)
s = base.getvector(s)
ss = np.stack((s, s), axis=-1)
cc = tri[0:, 0:2] / ss
elif tri.ndim == 3 and tri.shape[-1] == 3:
# N x M x 3 case
# tri is given as an image
s = np.sum(tri, axis=2)
ss = np.stack((s, s), axis=-1) # could also use np.tile
cc = tri[0:, 0:, :2] / ss
elif base.isvector(tri, 3):
tri = base.getvector(tri)
cc = tri[:2] / tri[2]
else:
raise ValueError('bad shape input')
return cc
def __init__(self, arg=None, w=None, check=True):
"""
Construct a new Twist object
TW = Twist(T) is a Twist object representing the SE(2) or SE(3)
homogeneous transformation matrix T (3x3 or 4x4).
TW = Twist(V) is a twist object where the vector is specified directly.
3D CASE:
TW = Twist('R', A, Q) is a Twist object representing rotation about the
axis of direction A (3x1) and passing through the point Q (3x1).
%
TW = Twist('R', A, Q, P) as above but with a pitch of P (distance/angle).
TW = Twist('T', A) is a Twist object representing translation in the
direction of A (3x1).
Notes:
- The argument 'P' for prismatic is synonymous with 'T'.
"""
# super().__init__() # enable UserList superpowers
if w is None:
# zero or one arguments passed
if super().arghandler(arg, convertfrom=(SE3, ), check=check):
return
elif base.isvector(arg, 6):
# Twist(array_like)
self.data = [base.getvector(arg)]
return
elif w is not None and base.isvector(w, 3) and base.isvector(arg, 3):
# Twist(v, w)
self.data = [np.r_[arg, w]]
return
raise ValueError('bad twist value')
def homtrans(T, p):
r"""
Apply a homogeneous transformation to a Euclidean vector
:param T: homogeneous transformation
:type T: Numpy array (3,3) or (4,4)
:param p: Vector(s) to be transformed
:type p: Numpy array (2,), (2,N), (3,) or (3,N)
:return: transformed Euclidean vector(s)
:rtype: Numpy array (2,), (2,N), (3,) or (3,N)
:raises ValueError: bad argument
``homtrans(T, p)`` applies the homogeneous transformation ``T`` to the points
stored columnwise in ``p``.
- If ``T`` is in SE(2) (3x3) and
- ``p`` is 2xN (2D points) they are considered Euclidean (:math:`\mathbb{R}^2`)
- ``p`` is 3xN (2D points) they are considered projective (:math:`\mathbb{P}^2`)
- If ``T`` is in SE(3) (4x4) and
- ``p`` is 3xN (3D points) they are considered Euclidean (:math:`\mathbb{R}^3`)
- ``p`` is 4xN (3D points) they are considered projective (:math:`\mathbb{P}^3`)
The return value and ``p`` have the same number of rows, ie. if Euclidean points are given
then Euclidean points are returned, if projective points are given then
projective points are returned.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> T = trotx(0.3)
>>> v = [1, 2, 3]
>>> h2e( T @ e2h(v))
>>> homtrans(T, v)
.. note::
- If T is a homogeneous transformation defining the pose of {B} with respect to {A},
then the points are defined with respect to frame {B} and are transformed to be
with respect to frame {A}.
:seealso: :func:`e2h`, :func:`h2e`
"""
if base.isvector(p):
p = base.getvector(p)
if p.shape[0] == T.shape[0] - 1:
# Euclidean vector
return h2e(T @ e2h(p))
elif p.shape[0] == T.shape[0]:
# homogeneous vector
return T @ p
else:
raise ValueError('matrices and point data do not conform')
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 eval_control(self, control, x):
"""
Evaluate vehicle control input (superclass method)
:param control: vehicle control
:type control: [type]
:param x: vehicle state :math:`(x, y, \theta)`
:type x: array_like(3)
:raises ValueError: bad control
:return: vehicle control inputs
:rtype: ndarray(2)
Evaluates the control for this time step and state. Control can be:
* a constant 2-tuple as the control inputs to the vehicle
* a function called as ``f(vehicle, t, x)`` that returns a tuple
* an interpolator called as f(t) that returns a tuple, see
SciPy interp1d
* a ``VehicleDriver`` subclass object
.. note:: Vehicle steering, speed and acceleration limits are applied.
"""
# was called control() in the MATLAB version
if base.isvector(control, 2):
# control is a constant
u = base.getvector(control, 2)
elif isinstance(control, VehicleDriver):
# vehicle has a driver object
u = control.demand()
elif isinstance(control, interpolate.interpolate.interp1d):
# control is an interp1d object
u = control(self._t)
elif callable(control):
# control is a user function of time and state
u = control(self, self._t, x)
else:
raise ValueError('bad control specified')
# apply limits
ulim = self.u_limited(u)
return ulim
def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:rtype: SE3 instance
There are multiple call signatures:
- ``SE3()`` is an ``SE3`` instance with one value -- a 4x4 identity
matrix which corresponds to a null motion.
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` is an ``SE3`` instance with the value ``T`` which is a 4x4
numpy array representing an SE(3) matrix. If ``check`` is ``True``
check the matrix belongs to SE(3).
- ``SE3(X)`` is an ``SE3`` instance with the same value as ``X``, ie.
a copy.
- ``SE3([T1, T2, ... TN])`` is an ``SE3`` instance with ``N`` values
given by the elements ``Ti`` each of which is a 4x4 NumPy array
representing an SE(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SE(3).
- ``SE3([X1, X2, ... XN])`` is an ``SE3`` instance with ``N`` values
given by the elements ``Xi`` each of which is an SE3 instance.
:SymPy: supported
"""
if y is None and z is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif base.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [base.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [base.transl(T) for T in x]
else:
raise ValueError('bad argument to constructor')
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [base.transl(x, y, z)]
def Exp(cls, S, check=True):
"""
Create an SE(3) matrix from se(3)
:param S: Lie algebra se(3) matrix
:type S: numpy ndarray
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Exp(S)`` is an SE(3) rotation defined by its Lie algebra
which is a 4x4 se(3) matrix (skew symmetric)
- ``SE3.Exp(t)`` is an SE(3) rotation defined by a 6-element twist
vector (the unique elements of the se(3) skew-symmetric matrix)
:seealso: :func:`~spatialmath.base.transforms3d.trexp`, :func:`~spatialmath.base.transformsNd.skew`
"""
if base.isvector(S, 6):
return cls(base.trexp(base.getvector(S)), check=False)
else:
return cls([base.trexp(s) for s in S], check=False)
def Eul(cls, *angles, unit='rad'):
r"""
Create an SE(3) pure rotation from Euler angles
:param 𝚪: Euler angles
:type 𝚪: array_like or numpy.ndarray with shape=(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Eul(𝚪)`` is an SE(3) rotation defined by a 3-vector of Euler
angles :math:`\Gamma=(\phi, \theta, \psi)` which correspond to
consecutive rotations about the Z, Y, Z axes respectively.
If ``𝚪`` is an Nx3 matrix then the result is a sequence of
rotations each defined by Euler angles corresponding to the rows of
``𝚪``.
- ``SE3.Eul(φ, θ, ψ)`` as above but the angles are provided as three
scalars.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Eul(0.1, 0.2, 0.3)
>>> SE3.Eul([0.1, 0.2, 0.3])
>>> SE3.Eul(10, 20, 30, unit='deg')
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.base.transforms3d.eul2r`
:SymPy: supported
"""
if len(angles) == 1:
angles = angles[0]
if base.isvector(angles, 3):
return cls(base.eul2tr(angles, unit=unit), check=False)
else:
return cls([base.eul2tr(a, unit=unit) for a in angles],
check=False)
def __init__(self, real=None, dual=None):
"""
Construct a new dual quaternion
:param real: real quaternion
:type real: Quaternion or UnitQuaternion
:param dual: dual quaternion
:type dual: Quaternion or UnitQuaternion
:raises ValueError: incorrect parameters
Example:
.. runblock:: pycon
>>> from spatialmath import DualQuaternion, Quaternion
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
>>> print(d)
>>> d = DualQuaternion([1, 2, 3, 4, 5, 6, 7, 8])
>>> print(d)
The dual number is stored internally as two quaternion, respectively
called ``real`` and ``dual``.
"""
if real is None and dual is None:
self.real = None
self.dual = None
return
elif dual is None and base.isvector(real, 8):
self.real = Quaternion(real[0:4])
self.dual = Quaternion(real[4:8])
elif real is not None and dual is not None:
if not isinstance(real, Quaternion):
raise ValueError('real part must be a Quaternion subclass')
if not isinstance(dual, Quaternion):
raise ValueError('real part must be a Quaternion subclass')
self.real = real # quaternion, real part
self.dual = dual # quaternion, dual part
else:
raise ValueError('expecting zero or two parameters')
def __init__(self, arg=None, w=None, check=True):
r"""
Construct a new 2D Twist object
:type a: 2-element array-like
:return: 2D prismatic twist
:rtype: Twist2 instance
- ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as
a 2x2 matrix.
- ``Twist2(T)`` is a 2D Twist object representing the SE(2) rigid-body motion expressed as
a 3x3 matrix.
- ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation,
and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion
- ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the
moment vector (1 element) and direction vector (2 elements).
:References:
- **Robotics, Vision & Control**, Corke, Springer 2017.
- **Modern Robotics, Lynch & Park**, Cambridge 2017
.. note:: Compared to Lynch & Park this module implements twist vectors
with the translational components first, followed by rotational
components, ie. :math:`[\omega, \vec{v}]`.
"""
super().__init__()
if w is None:
# zero or one arguments passed
if super().arghandler(arg, convertfrom=(SE2, ), check=check):
return
elif w is not None and base.isscalar(w) and base.isvector(arg, 2):
# Twist(v, w)
self.data = [np.r_[arg, w]]
return
raise ValueError('bad twist value')
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 RPY(cls, angles, *, order='zyx', unit='rad'):
r"""
Construct a new SO(3) from roll-pitch-yaw angles
:param angles: roll-pitch-yaw angles
:type angles: array_like or numpy.ndarray with shape=(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type order: str
:return: SO(3) rotation
:rtype: SO3 instance
``SO3.RPY(angles)`` is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles :math:`(r, p, y)`
which correspond to successive rotations about the axes specified by ``order``:
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
and y-axis sideways.
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll about the new z-axis. Convention for a camera with z-axis parallel
to the optic axis and x-axis parallel to the pixel rows.
If ``angles`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
corresponding to the rows of angles.
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r`
"""
if base.isvector(angles, 3):
return cls(base.rpy2r(angles, order=order, unit=unit), check=False)
else:
return cls([base.rpy2r(a, order=order, unit=unit) for a in angles],
check=False)
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")
