Classes and functions¶
Pose classes¶
Pose in 2D¶
-
class
spatialmath.pose2d.
SO2
(arg=None, *, unit='rad', check=True)[source]¶ Bases:
spatialmath.super_pose.SMPose
-
__init__
(arg=None, *, unit='rad', check=True)[source]¶ Construct new SO(2) object
- Parameters
unit (str, optional) – angular units ‘deg’ or ‘rad’ [default] if applicable
check (bool) – check for valid SO(2) elements if applicable, default to True
- Returns
SO(2) rotation
- Return type
SO2 instance
SO2()
is an SO2 instance representing a null rotation – the identity matrix.SO2(theta)
is an SO2 instance representing a rotation bytheta
radians. Iftheta
is array_like [theta1, theta2, … thetaN] then an SO2 instance containing a sequence of N rotations.SO2(theta, unit='deg')
is an SO2 instance representing a rotation bytheta
degrees. Iftheta
is array_like [theta1, theta2, … thetaN] then an SO2 instance containing a sequence of N rotations.SO2(R)
is an SO2 instance with rotation described by the SO(2) matrix R which is a 2x2 numpy array. Ifcheck
isTrue
check the matrix belongs to SO(2).SO2([R1, R2, ... RN])
is an SO2 instance containing a sequence of N rotations, each described by an SO(2) matrix Ri which is a 2x2 numpy array. Ifcheck
isTrue
then check each matrix belongs to SO(2).SO2([X1, X2, ... XN])
is an SO2 instance containing a sequence of N rotations, where each Xi is an SO2 instance.
-
classmethod
Rand
(*, range=[0, 6.283185307179586], unit='rad', N=1)[source]¶ Construct new SO(2) with random rotation
- Parameters
range (2-element array-like, optional) – rotation range, defaults to \([0, 2\pi)\).
unit (str, optional) – angular units as ‘deg or ‘rad’ [default]
N (int) – number of random rotations, defaults to 1
- Returns
SO(2) rotation matrix
- Return type
SO2 instance
SO2.Rand()
is a random SO(2) rotation.SO2.Rand([-90, 90], unit='deg')
is a random SO(2) rotation between -90 and +90 degrees.SO2.Rand(N)
is a sequence of N random rotations.
Rotations are uniform over the specified interval.
-
classmethod
Exp
(S, check=True)[source]¶ Construct new SO(2) rotation matrix from so(2) Lie algebra
- Parameters
S (numpy ndarray) – element of Lie algebra so(2)
check (bool) – check that passed matrix is valid so(2), default True
- Returns
SO(2) rotation matrix
- Return type
SO2 instance
SO2.Exp(S)
is an SO(2) rotation defined by its Lie algebra which is a 2x2 so(2) matrix (skew symmetric)
- Seealso
spatialmath.base.transforms2d.trexp()
,spatialmath.base.transformsNd.skew()
-
static
isvalid
(x)[source]¶ Test if matrix is valid SO(2)
- Parameters
x (numpy.ndarray) – matrix to test
- Returns
True if the matrix is a valid element of SO(2), ie. it is a 2x2 orthonormal matrix with determinant of +1.
- Return type
bool
- Seealso
isrot()
-
inv
()[source]¶ Inverse of SO(2)
- Returns
inverse rotation
- Return type
SO2 instance
x.inv()
is the inverse of x.
Notes:
for elements of SO(2) this is the transpose.
if x contains a sequence, returns an SO2 with a sequence of inverses
-
property
R
¶ SO(2) or SE(2) as rotation matrix
- Returns
rotational component
- Return type
numpy.ndarray, shape=(2,2)
x.R
returns the rotation matrix, when x is SO2 or SE2. If len(x) is:1, return an ndarray with shape=(2,2)
N>1, return ndarray with shape=(N,2,2)
-
theta
(units='rad')[source]¶ SO(2) as a rotation angle
- Parameters
unit (str, optional) – angular units ‘deg’ or ‘rad’ [default]
- Returns
rotation angle
- Return type
float or list
x.theta
is the rotation angle such that x is SO2(x.theta).
-
SE2
()[source]¶ Create SE(2) from SO(2)
- Returns
SE(2) with same rotation but zero translation
- Return type
SE2 instance
-
property
A
¶ Interal array representation (superclass property)
- Parameters
- Returns
The numeric array
- Return type
numpy.ndarray
Each pose subclass SO(N) or SE(N) are stored internally as a numpy array. This property returns the array, shape depends on the particular subclass.
Examples:
>>> x = SE3() >>> x.A array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
- Seealso
shape, N
-
classmethod
Empty
()¶ Construct a new pose object with zero items (superclass method)
- Parameters
- Returns
a pose with zero values
- Return type
This constructs an empty pose container which can be appended to. For example:
>>> x = SO2.Empty() >>> len(x) 0 >>> x.append(SO2(20, 'deg')) >>> len(x) 1
-
property
N
¶ Dimension of the object’s group (superclass property)
- Returns
dimension
- Return type
int
Dimension of the group is 2 for
SO2
orSE2
, and 3 forSO3
orSE3
. This corresponds to the dimension of the space, 2D or 3D, to which these rotations or rigid-body motions apply.Example:
>>> x = SE3() >>> x.N 3
-
__add__
(right)¶ Overloaded
+
operator (superclass method)- Parameters
left – left addend
right – right addend
- Returns
sum
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray, shape=(N,N)
Add elements of two poses. This is not a group operation so the result is a matrix not a pose class.
X + Y
is the element-wise sum of the matrix value ofX
andY
X + s
is the element-wise sum of the matrix value ofX
ands
s + X
is the element-wise sum of the matrix value ofs
andX
Operands
Sum
left
right
type
operation
Pose
Pose
NxN matrix
element-wise matrix sum
Pose
scalar
NxN matrix
element-wise sum
scalar
Pose
NxN matrix
element-wise sum
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar + Pose is handled by
__radd__
scalar addition is commutative
Any other input combinations result in a ValueError.
For pose addition the
left
andright
operands may be a sequence which results in the result being a sequence:len(left)
len(right)
len
operation
1
1
1
prod = left + right
1
M
M
prod[i] = left + right[i]
N
1
M
prod[i] = left[i] + right
M
M
M
prod[i] = left[i] + right[i]
-
__eq__
(right)¶ Overloaded
==
operator (superclass method)- Parameters
left – left side of comparison
right – right side of comparison
- Returns
poses are equal
- Return type
bool
Test two poses for equality
X == Y
is true of the poses are of the same type and numerically equal.
If either operand contains a sequence the results is a sequence according to:
len(left)
len(right)
len
operation
1
1
1
ret = left == right
1
M
M
ret[i] = left == right[i]
N
1
M
ret[i] = left[i] == right
M
M
M
ret[i] = left[i] == right[i]
-
__mul__
(right)¶ Overloaded
*
operator (superclass method)- Parameters
left – left multiplicand
right – right multiplicand
- Returns
product
- Raises
ValueError
Pose composition, scaling or vector transformation:
X * Y
compounds the posesX
andY
X * s
performs elementwise multiplication of the elements ofX
bys
s * X
performs elementwise multiplication of the elements ofX
bys
X * v
linear transform of the vectorv
Multiplicands
Product
left
right
type
operation
Pose
Pose
Pose
matrix product
Pose
scalar
NxN matrix
element-wise product
scalar
Pose
NxN matrix
element-wise product
Pose
N-vector
N-vector
vector transform
Pose
NxM matrix
NxM matrix
transform each column
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar x Pose is handled by
__rmul__
scalar multiplication is commutative but the result is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.
For pose composition the
left
andright
operands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right
1
M
M
prod[i] = left * right[i]
N
1
M
prod[i] = left[i] * right
M
M
M
prod[i] = left[i] * right[i]
For vector transformation there are three cases
Multiplicands
Product
len(left)
right.shape
shape
operation
1
(N,)
(N,)
vector transformation
M
(N,)
(N,M)
vector transformations
1
(N,M)
(N,M)
column transformation
Notes:
for the
SE2
andSE3
case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.
-
__ne__
(right)¶ Overloaded
!=
operator- Parameters
left – left side of comparison
right – right side of comparison
- Returns
poses are not equal
- Return type
bool
Test two poses for inequality
X == Y
is true of the poses are of the same type but not numerically equal.
If either operand contains a sequence the results is a sequence according to:
len(left)
len(right)
len
operation
1
1
1
ret = left != right
1
M
M
ret[i] = left != right[i]
N
1
M
ret[i] = left[i] != right
M
M
M
ret[i] = left[i] != right[i]
-
__pow__
(n)¶ Overloaded
**
operator (superclass method)- Parameters
n – pose
- Returns
pose to the power n
Raise all elements of pose to the specified power.
X**n
raise all values inX
to the powern
-
__sub__
(right)¶ Overloaded
-
operator (superclass method)- Parameters
left – left minuend
right – right subtrahend
- Returns
difference
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray, shape=(N,N)
Subtract elements of two poses. This is not a group operation so the result is a matrix not a pose class.
X - Y
is the element-wise difference of the matrix value ofX
andY
X - s
is the element-wise difference of the matrix value ofX
ands
s - X
is the element-wise difference ofs
and the matrix value ofX
Operands
Sum
left
right
type
operation
Pose
Pose
NxN matrix
element-wise matrix difference
Pose
scalar
NxN matrix
element-wise sum
scalar
Pose
NxN matrix
element-wise sum
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar - Pose is handled by
__rsub__
Any other input combinations result in a ValueError.
For pose addition the
left
andright
operands may be a sequence which results in the result being a sequence:len(left)
len(right)
len
operation
1
1
1
prod = left - right
1
M
M
prod[i] = left - right[i]
N
1
M
prod[i] = left[i] - right
M
M
M
prod[i] = left[i] right[i]
-
__truediv__
(right)¶ Overloaded
/
operator (superclass method)- Parameters
left – left multiplicand
right – right multiplicand
- Returns
product
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray
Pose composition or scaling:
X / Y
compounds the posesX
andY.inv()
X / s
performs elementwise multiplication of the elements ofX
bys
Multiplicands
Quotient
left
right
type
operation
Pose
Pose
Pose
matrix product by inverse
Pose
scalar
NxN matrix
element-wise division
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar multiplication is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.
For pose composition the
left
andright
operands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right.inv()
1
M
M
prod[i] = left * right[i].inv()
N
1
M
prod[i] = left[i] * right.inv()
M
M
M
prod[i] = left[i] * right[i].inv()
-
property
about
¶ Succinct summary of object type and length (superclass property)
- Returns
succinct summary
- Return type
str
Displays the type and the number of elements in compact form, for example:
>>> x = SE3([SE3() for i in range(20)]) >>> len(x) 20 >>> print(x.about) SE3[20]
-
animate
(*args, T0=None, **kwargs)¶ Plot pose object as an animated coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
as a coordinate frame moving from the origin, orT0
, in either 2D or 3D axes. There are many options, see the links below.
Example:
>>> X = SE3.Rx(0.3) >>> X.animate(frame='A', color='green')
- Seealso
-
append
(x)¶ Append a value to a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of appended object
Appends the argument to the object’s internal list of values.
Examples:
>>> x = SE3() >>> len(x) 1 >>> x.append(SE3.Rx(0.1)) >>> len(x) 2
-
clear
() → None -- remove all items from S¶
-
extend
(x)¶ Extend sequence of values of a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of appended object
Appends the argument to the object’s internal list of values.
Examples:
>>> x = SE3() >>> len(x) 1 >>> x.append(SE3.Rx(0.1)) >>> len(x) 2
-
insert
(i, value)¶ Insert a value to a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of inserted value
Inserts the argument into the object’s internal list of values.
Examples:
>>> x = SE3() >>> x.inert(0, SE3.Rx(0.1)) # insert at position 0 in the list >>> len(x) 2
-
interp
(s=None, T0=None)¶ Interpolate pose (superclass method)
- Parameters
- Returns
interpolated pose
- Return type
X.interp(s)
interpolates the pose X between identity when s=0 and X when s=1.
len(X)
len(s)
len(result)
Result
1
1
1
Y = interp(identity, X, s)
M
1
M
Y[i] = interp(T0, X[i], s)
1
M
M
Y[i] = interp(T0, X, s[i])
Example:
>>> x = SE3.Rx(0.3) >>> print(x.interp(0)) SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])) >>> print(x.interp(1)) SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.95533649, -0.29552021, 0. ], [ 0. , 0.29552021, 0.95533649, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> y = x.interp(x, np.linspace(0, 1, 10)) >>> len(y) 10 >>> y[5] SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.98614323, -0.16589613, 0. ], [ 0. , 0.16589613, 0.98614323, 0. ], [ 0. , 0. , 0. , 1. ]]))
Notes:
For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
-
property
isSE
¶ Test if object belongs to SE(n) group (superclass property)
-
property
isSO
¶ Test if object belongs to SO(n) group (superclass property)
-
ishom
()¶ Test if object belongs to SE(3) group (superclass method)
- Returns
True
if object is instance of SE3- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SE3)
.Example:
>>> x = SO3() >>> x.isrot() False >>> x = SE3() >>> x.isrot() True
-
ishom2
()¶ Test if object belongs to SE(2) group (superclass method)
- Returns
True
if object is instance of SE2- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SE2)
.Example:
>>> x = SO2() >>> x.isrot() False >>> x = SE2() >>> x.isrot() True
-
isrot
()¶ Test if object belongs to SO(3) group (superclass method)
- Returns
True
if object is instance of SO3- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SO3)
.Example:
>>> x = SO3() >>> x.isrot() True >>> x = SE3() >>> x.isrot() False
-
isrot2
()¶ Test if object belongs to SO(2) group (superclass method)
- Returns
True
if object is instance of SO2- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SO2)
.Example:
>>> x = SO2() >>> x.isrot() True >>> x = SE2() >>> x.isrot() False
-
log
()¶ Logarithm of pose (superclass method)
- Returns
logarithm
- Return type
numpy.ndarray
- Raises
ValueError
An efficient closed-form solution of the matrix logarithm.
Input
Output
Pose
Shape
Structure
SO2
(2,2)
skew-symmetric
SE2
(3,3)
augmented skew-symmetric
SO3
(3,3)
skew-symmetric
SE3
(4,4)
augmented skew-symmetric
Example:
>>> x = SE3.Rx(0.3) >>> y = x.log() >>> y array([[ 0. , -0. , 0. , 0. ], [ 0. , 0. , -0.3, 0. ], [-0. , 0.3, 0. , 0. ], [ 0. , 0. , 0. , 0. ]])
-
norm
()¶ Normalize pose (superclass method)
X.norm()
is an equivalent pose object but the rotational matrix part of all values has been adjusted to ensure it is a proper orthogonal matrix rotation.
Example:
>>> x = SE3() >>> y = x.norm() >>> y SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]))
Notes:
Only the direction of A vector (the z-axis) is unchanged.
Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.
- Seealso
trnorm()
,trnorm2()
-
plot
(*args, **kwargs)¶ Plot pose object as a coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
as a coordinate frame in either 2D or 3D axes. There are many options, see the links below.
Example:
>>> X = SE3.Rx(0.3) >>> X.plot(frame='A', color='green')
-
pop
()¶ Pop value of a pose object (superclass method)
- Returns
the specific element of the pose
- Return type
- Raises
IndexError – if there are no values to pop
Removes the first pose value from the sequence in the pose object.
Example:
>>> x = SE3.Rx([0, math.pi/2, math.pi]) >>> len(x) 3 >>> y = x.pop() >>> y SE3(array([[ 1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00], [ 0.0000000e+00, -1.0000000e+00, -1.2246468e-16, 0.0000000e+00], [ 0.0000000e+00, 1.2246468e-16, -1.0000000e+00, 0.0000000e+00], [ 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00]])) >>> len(x) 2
-
printline
(**kwargs)¶ Print pose as a single line (superclass method)
- Parameters
label (str) – text label to put at start of line
file (str) – file to write formatted string to. [default, stdout]
fmt (str) – conversion format for each number as used by
format()
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
optional formatted string
- Return type
str
For SO(3) or SE(3) also:
- Parameters
orient (str) – 3-angle convention to use
X.printline()
printX
in single-line format tostdout
, followed by a newlineX.printline(file=None)
return a string containingX
in single-line format
Example:
>>> x=SE3.Rx(0.3) >>> x.printline() t = 0, 0, 0; rpy/zyx = 17, 0, 0 deg
-
reverse
()¶ S.reverse() – reverse IN PLACE
-
property
shape
¶ Shape of the object’s matrix representation (superclass property)
- Returns
matrix shape
- Return type
2-tuple of ints
(2,2) for
SO2
, (3,3) forSE2
andSO3
, and (4,4) forSE3
.Example:
>>> x = SE3() >>> x.shape (4, 4)
-
-
class
spatialmath.pose2d.
SE2
(x=None, y=None, theta=None, *, unit='rad', check=True)[source]¶ Bases:
spatialmath.pose2d.SO2
-
__init__
(x=None, y=None, theta=None, *, unit='rad', check=True)[source]¶ Construct new SE(2) object
- Parameters
unit (str, optional) – angular units ‘deg’ or ‘rad’ [default] if applicable
check (bool) – check for valid SE(2) elements if applicable, default to True
- Returns
homogeneous rigid-body transformation matrix
- Return type
SE2 instance
SE2()
is an SE2 instance representing a null motion – the identity matrixSE2(x, y)
is an SE2 instance representing a pure translation of (x
,y
)SE2(t)
is an SE2 instance representing a pure translation of (x
,y
) where``t``=[x,y] is a 2-element array_likeSE2(x, y, theta)
is an SE2 instance representing a translation of (x
,y
) and a rotation oftheta
radiansSE2(x, y, theta, unit='deg')
is an SE2 instance representing a translation of (x
,y
) and a rotation oftheta
degreesSE2(t)
is an SE2 instance representing a translation of (x
,y
) and a rotation oftheta
where ``t``=[x,y,theta] is a 3-element array_likeSE2(T)
is an SE2 instance with rigid-body motion described by the SE(2) matrix T which is a 3x3 numpy array. Ifcheck
isTrue
check the matrix belongs to SE(2).SE2([T1, T2, ... TN])
is an SE2 instance containing a sequence of N rigid-body motions, each described by an SE(2) matrix Ti which is a 3x3 numpy array. Ifcheck
isTrue
then check each matrix belongs to SE(2).SE2([X1, X2, ... XN])
is an SE2 instance containing a sequence of N rigid-body motions, where each Xi is an SE2 instance.
-
classmethod
Rand
(*, xrange=[-1, 1], yrange=[-1, 1], trange=[0, 6.283185307179586], unit='rad', N=1)[source]¶ Construct a new random SE(2)
- Parameters
xrange (2-element sequence, optional) – x-axis range [min,max], defaults to [-1, 1]
yrange (2-element sequence, optional) – y-axis range [min,max], defaults to [-1, 1]
trange – theta range [min,max], defaults to \([0, 2\pi)\)
N (int) – number of random rotations, defaults to 1
- Returns
homogeneous rigid-body transformation matrix
- Return type
SE2 instance
Return an SE2 instance with random rotation and translation.
SE2.Rand()
is a random SE(2) rotation.SE2.Rand(N)
is an SE2 object containing a sequence of N random poses.
Example, create random ten vehicles in the xy-plane:
>>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) >>> len(x) 10
-
classmethod
Exp
(S, check=True, se2=True)[source]¶ Construct a new SE(2) from se(2) Lie algebra
- Parameters
S (numpy ndarray) – element of Lie algebra se(2)
check (bool) – check that passed matrix is valid se(2), default True
se2 (bool) – input is an se(2) matrix (default True)
- Returns
homogeneous transform matrix
- Return type
SE2 instance
SE2.Exp(S)
is an SE(2) rotation defined by its Lie algebra which is a 3x3 se(2) matrix (skew symmetric)SE2.Exp(t)
is an SE(2) rotation defined by a 3-element twist vector array_like (the unique elements of the se(2) skew-symmetric matrix)SE2.Exp(T)
is a sequence of SE(2) rigid-body motions defined by an Nx3 matrix of twist vectors, one per row.
Note:
an input 3x3 matrix is ambiguous, it could be the first or third case above. In this case the argument
se2
is the decider.
- Seealso
spatialmath.base.transforms2d.trexp()
,spatialmath.base.transformsNd.skew()
-
static
isvalid
(x)[source]¶ Test if matrix is valid SE(2)
- Parameters
x (numpy.ndarray) – matrix to test
- Returns
true if the matrix is a valid element of SE(2), ie. it is a 3x3 homogeneous rigid-body transformation matrix.
- Return type
bool
- Seealso
ishom()
-
property
A
¶ Interal array representation (superclass property)
- Parameters
- Returns
The numeric array
- Return type
numpy.ndarray
Each pose subclass SO(N) or SE(N) are stored internally as a numpy array. This property returns the array, shape depends on the particular subclass.
Examples:
>>> x = SE3() >>> x.A array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
- Seealso
shape, N
-
classmethod
Empty
()¶ Construct a new pose object with zero items (superclass method)
- Parameters
- Returns
a pose with zero values
- Return type
This constructs an empty pose container which can be appended to. For example:
>>> x = SO2.Empty() >>> len(x) 0 >>> x.append(SO2(20, 'deg')) >>> len(x) 1
-
property
N
¶ Dimension of the object’s group (superclass property)
- Returns
dimension
- Return type
int
Dimension of the group is 2 for
SO2
orSE2
, and 3 forSO3
orSE3
. This corresponds to the dimension of the space, 2D or 3D, to which these rotations or rigid-body motions apply.Example:
>>> x = SE3() >>> x.N 3
-
property
R
¶ SO(2) or SE(2) as rotation matrix
- Returns
rotational component
- Return type
numpy.ndarray, shape=(2,2)
x.R
returns the rotation matrix, when x is SO2 or SE2. If len(x) is:1, return an ndarray with shape=(2,2)
N>1, return ndarray with shape=(N,2,2)
-
SE2
()¶ Create SE(2) from SO(2)
- Returns
SE(2) with same rotation but zero translation
- Return type
SE2 instance
-
__add__
(right)¶ Overloaded
+
operator (superclass method)- Parameters
left – left addend
right – right addend
- Returns
sum
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray, shape=(N,N)
Add elements of two poses. This is not a group operation so the result is a matrix not a pose class.
X + Y
is the element-wise sum of the matrix value ofX
andY
X + s
is the element-wise sum of the matrix value ofX
ands
s + X
is the element-wise sum of the matrix value ofs
andX
Operands
Sum
left
right
type
operation
Pose
Pose
NxN matrix
element-wise matrix sum
Pose
scalar
NxN matrix
element-wise sum
scalar
Pose
NxN matrix
element-wise sum
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar + Pose is handled by
__radd__
scalar addition is commutative
Any other input combinations result in a ValueError.
For pose addition the
left
andright
operands may be a sequence which results in the result being a sequence:len(left)
len(right)
len
operation
1
1
1
prod = left + right
1
M
M
prod[i] = left + right[i]
N
1
M
prod[i] = left[i] + right
M
M
M
prod[i] = left[i] + right[i]
-
__eq__
(right)¶ Overloaded
==
operator (superclass method)- Parameters
left – left side of comparison
right – right side of comparison
- Returns
poses are equal
- Return type
bool
Test two poses for equality
X == Y
is true of the poses are of the same type and numerically equal.
If either operand contains a sequence the results is a sequence according to:
len(left)
len(right)
len
operation
1
1
1
ret = left == right
1
M
M
ret[i] = left == right[i]
N
1
M
ret[i] = left[i] == right
M
M
M
ret[i] = left[i] == right[i]
-
__mul__
(right)¶ Overloaded
*
operator (superclass method)- Parameters
left – left multiplicand
right – right multiplicand
- Returns
product
- Raises
ValueError
Pose composition, scaling or vector transformation:
X * Y
compounds the posesX
andY
X * s
performs elementwise multiplication of the elements ofX
bys
s * X
performs elementwise multiplication of the elements ofX
bys
X * v
linear transform of the vectorv
Multiplicands
Product
left
right
type
operation
Pose
Pose
Pose
matrix product
Pose
scalar
NxN matrix
element-wise product
scalar
Pose
NxN matrix
element-wise product
Pose
N-vector
N-vector
vector transform
Pose
NxM matrix
NxM matrix
transform each column
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar x Pose is handled by
__rmul__
scalar multiplication is commutative but the result is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.
For pose composition the
left
andright
operands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right
1
M
M
prod[i] = left * right[i]
N
1
M
prod[i] = left[i] * right
M
M
M
prod[i] = left[i] * right[i]
For vector transformation there are three cases
Multiplicands
Product
len(left)
right.shape
shape
operation
1
(N,)
(N,)
vector transformation
M
(N,)
(N,M)
vector transformations
1
(N,M)
(N,M)
column transformation
Notes:
for the
SE2
andSE3
case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.
-
__ne__
(right)¶ Overloaded
!=
operator- Parameters
left – left side of comparison
right – right side of comparison
- Returns
poses are not equal
- Return type
bool
Test two poses for inequality
X == Y
is true of the poses are of the same type but not numerically equal.
If either operand contains a sequence the results is a sequence according to:
len(left)
len(right)
len
operation
1
1
1
ret = left != right
1
M
M
ret[i] = left != right[i]
N
1
M
ret[i] = left[i] != right
M
M
M
ret[i] = left[i] != right[i]
-
__pow__
(n)¶ Overloaded
**
operator (superclass method)- Parameters
n – pose
- Returns
pose to the power n
Raise all elements of pose to the specified power.
X**n
raise all values inX
to the powern
-
__sub__
(right)¶ Overloaded
-
operator (superclass method)- Parameters
left – left minuend
right – right subtrahend
- Returns
difference
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray, shape=(N,N)
Subtract elements of two poses. This is not a group operation so the result is a matrix not a pose class.
X - Y
is the element-wise difference of the matrix value ofX
andY
X - s
is the element-wise difference of the matrix value ofX
ands
s - X
is the element-wise difference ofs
and the matrix value ofX
Operands
Sum
left
right
type
operation
Pose
Pose
NxN matrix
element-wise matrix difference
Pose
scalar
NxN matrix
element-wise sum
scalar
Pose
NxN matrix
element-wise sum
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar - Pose is handled by
__rsub__
Any other input combinations result in a ValueError.
For pose addition the
left
andright
operands may be a sequence which results in the result being a sequence:len(left)
len(right)
len
operation
1
1
1
prod = left - right
1
M
M
prod[i] = left - right[i]
N
1
M
prod[i] = left[i] - right
M
M
M
prod[i] = left[i] right[i]
-
__truediv__
(right)¶ Overloaded
/
operator (superclass method)- Parameters
left – left multiplicand
right – right multiplicand
- Returns
product
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray
Pose composition or scaling:
X / Y
compounds the posesX
andY.inv()
X / s
performs elementwise multiplication of the elements ofX
bys
Multiplicands
Quotient
left
right
type
operation
Pose
Pose
Pose
matrix product by inverse
Pose
scalar
NxN matrix
element-wise division
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar multiplication is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.
For pose composition the
left
andright
operands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right.inv()
1
M
M
prod[i] = left * right[i].inv()
N
1
M
prod[i] = left[i] * right.inv()
M
M
M
prod[i] = left[i] * right[i].inv()
-
property
about
¶ Succinct summary of object type and length (superclass property)
- Returns
succinct summary
- Return type
str
Displays the type and the number of elements in compact form, for example:
>>> x = SE3([SE3() for i in range(20)]) >>> len(x) 20 >>> print(x.about) SE3[20]
-
animate
(*args, T0=None, **kwargs)¶ Plot pose object as an animated coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
as a coordinate frame moving from the origin, orT0
, in either 2D or 3D axes. There are many options, see the links below.
Example:
>>> X = SE3.Rx(0.3) >>> X.animate(frame='A', color='green')
- Seealso
-
append
(x)¶ Append a value to a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of appended object
Appends the argument to the object’s internal list of values.
Examples:
>>> x = SE3() >>> len(x) 1 >>> x.append(SE3.Rx(0.1)) >>> len(x) 2
-
clear
() → None -- remove all items from S¶
-
extend
(x)¶ Extend sequence of values of a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of appended object
Appends the argument to the object’s internal list of values.
Examples:
>>> x = SE3() >>> len(x) 1 >>> x.append(SE3.Rx(0.1)) >>> len(x) 2
-
insert
(i, value)¶ Insert a value to a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of inserted value
Inserts the argument into the object’s internal list of values.
Examples:
>>> x = SE3() >>> x.inert(0, SE3.Rx(0.1)) # insert at position 0 in the list >>> len(x) 2
-
interp
(s=None, T0=None)¶ Interpolate pose (superclass method)
- Parameters
- Returns
interpolated pose
- Return type
X.interp(s)
interpolates the pose X between identity when s=0 and X when s=1.
len(X)
len(s)
len(result)
Result
1
1
1
Y = interp(identity, X, s)
M
1
M
Y[i] = interp(T0, X[i], s)
1
M
M
Y[i] = interp(T0, X, s[i])
Example:
>>> x = SE3.Rx(0.3) >>> print(x.interp(0)) SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])) >>> print(x.interp(1)) SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.95533649, -0.29552021, 0. ], [ 0. , 0.29552021, 0.95533649, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> y = x.interp(x, np.linspace(0, 1, 10)) >>> len(y) 10 >>> y[5] SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.98614323, -0.16589613, 0. ], [ 0. , 0.16589613, 0.98614323, 0. ], [ 0. , 0. , 0. , 1. ]]))
Notes:
For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
-
property
isSE
¶ Test if object belongs to SE(n) group (superclass property)
-
property
isSO
¶ Test if object belongs to SO(n) group (superclass property)
-
ishom
()¶ Test if object belongs to SE(3) group (superclass method)
- Returns
True
if object is instance of SE3- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SE3)
.Example:
>>> x = SO3() >>> x.isrot() False >>> x = SE3() >>> x.isrot() True
-
ishom2
()¶ Test if object belongs to SE(2) group (superclass method)
- Returns
True
if object is instance of SE2- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SE2)
.Example:
>>> x = SO2() >>> x.isrot() False >>> x = SE2() >>> x.isrot() True
-
isrot
()¶ Test if object belongs to SO(3) group (superclass method)
- Returns
True
if object is instance of SO3- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SO3)
.Example:
>>> x = SO3() >>> x.isrot() True >>> x = SE3() >>> x.isrot() False
-
isrot2
()¶ Test if object belongs to SO(2) group (superclass method)
- Returns
True
if object is instance of SO2- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SO2)
.Example:
>>> x = SO2() >>> x.isrot() True >>> x = SE2() >>> x.isrot() False
-
log
()¶ Logarithm of pose (superclass method)
- Returns
logarithm
- Return type
numpy.ndarray
- Raises
ValueError
An efficient closed-form solution of the matrix logarithm.
Input
Output
Pose
Shape
Structure
SO2
(2,2)
skew-symmetric
SE2
(3,3)
augmented skew-symmetric
SO3
(3,3)
skew-symmetric
SE3
(4,4)
augmented skew-symmetric
Example:
>>> x = SE3.Rx(0.3) >>> y = x.log() >>> y array([[ 0. , -0. , 0. , 0. ], [ 0. , 0. , -0.3, 0. ], [-0. , 0.3, 0. , 0. ], [ 0. , 0. , 0. , 0. ]])
-
norm
()¶ Normalize pose (superclass method)
X.norm()
is an equivalent pose object but the rotational matrix part of all values has been adjusted to ensure it is a proper orthogonal matrix rotation.
Example:
>>> x = SE3() >>> y = x.norm() >>> y SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]))
Notes:
Only the direction of A vector (the z-axis) is unchanged.
Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.
- Seealso
trnorm()
,trnorm2()
-
plot
(*args, **kwargs)¶ Plot pose object as a coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
as a coordinate frame in either 2D or 3D axes. There are many options, see the links below.
Example:
>>> X = SE3.Rx(0.3) >>> X.plot(frame='A', color='green')
-
pop
()¶ Pop value of a pose object (superclass method)
- Returns
the specific element of the pose
- Return type
- Raises
IndexError – if there are no values to pop
Removes the first pose value from the sequence in the pose object.
Example:
>>> x = SE3.Rx([0, math.pi/2, math.pi]) >>> len(x) 3 >>> y = x.pop() >>> y SE3(array([[ 1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00], [ 0.0000000e+00, -1.0000000e+00, -1.2246468e-16, 0.0000000e+00], [ 0.0000000e+00, 1.2246468e-16, -1.0000000e+00, 0.0000000e+00], [ 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00]])) >>> len(x) 2
-
printline
(**kwargs)¶ Print pose as a single line (superclass method)
- Parameters
label (str) – text label to put at start of line
file (str) – file to write formatted string to. [default, stdout]
fmt (str) – conversion format for each number as used by
format()
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
optional formatted string
- Return type
str
For SO(3) or SE(3) also:
- Parameters
orient (str) – 3-angle convention to use
X.printline()
printX
in single-line format tostdout
, followed by a newlineX.printline(file=None)
return a string containingX
in single-line format
Example:
>>> x=SE3.Rx(0.3) >>> x.printline() t = 0, 0, 0; rpy/zyx = 17, 0, 0 deg
-
reverse
()¶ S.reverse() – reverse IN PLACE
-
property
shape
¶ Shape of the object’s matrix representation (superclass property)
- Returns
matrix shape
- Return type
2-tuple of ints
(2,2) for
SO2
, (3,3) forSE2
andSO3
, and (4,4) forSE3
.Example:
>>> x = SE3() >>> x.shape (4, 4)
-
property
t
¶ Translational component of SE(2)
- Parameters
self (SE2 instance) – SE(2)
- Returns
translational component
- Return type
numpy.ndarray
x.t
is the translational vector component. Iflen(x)
is:1, return an ndarray with shape=(2,)
N>1, return an ndarray with shape=(N,2)
-
theta
(units='rad')¶ SO(2) as a rotation angle
- Parameters
unit (str, optional) – angular units ‘deg’ or ‘rad’ [default]
- Returns
rotation angle
- Return type
float or list
x.theta
is the rotation angle such that x is SO2(x.theta).
-
xyt
()[source]¶ SE(2) as a configuration vector
- Returns
An array \([x, y, \theta]\)
- Return type
numpy.ndarray
x.xyt
is the rigidbody motion in minimal form as a translation and rotation expressed in vector form as \([x, y, \theta]\). Iflen(x)
is:1, return an ndarray with shape=(3,)
N>1, return an ndarray with shape=(N,3)
-
inv
()[source]¶ Inverse of SE(2)
- Parameters
self (SE2 instance) – pose
- Returns
inverse
- Return type
Notes:
for elements of SE(2) this takes into account the matrix structure \(T^{-1} = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]\)
if x contains a sequence, returns an SE2 with a sequence of inverses
-
Pose in 3D¶
-
class
spatialmath.pose3d.
SO3
(arg=None, *, check=True)[source]¶ Bases:
spatialmath.super_pose.SMPose
SO(3) subclass
This subclass represents rotations in 3D space. Internally it is a 3x3 orthogonal matrix belonging to the group SO(3).
-
__init__
(arg=None, *, check=True)[source]¶ Construct new SO(3) object
SO3()
is an SO3 instance representing null rotation – the identity matrixSO3(R)
is an SO3 instance with rotation matrix R which is a 3x3 numpy array representing an valid rotation matrix. Ifcheck
isTrue
check the matrix value.SO3([R1, R2, ... RN])
where each Ri is a 3x3 numpy array of rotation matrices, is an SO3 instance containing N rotations. Ifcheck
isTrue
then each matrix is checked for validity.SO3([X1, X2, ... XN])
where each Xi is an SO3 instance, is an SO3 instance containing N rotations.
- Seealso
SMPose.pose_arghandler
-
property
R
¶ SO(3) or SE(3) as rotation matrix
- Returns
rotational component
- Return type
numpy.ndarray, shape=(3,3)
x.R
returns the rotation matrix, when x is SO3 or SE3. If len(x) is:1, return an ndarray with shape=(3,3)
N>1, return ndarray with shape=(N,3,3)
-
property
n
¶ Normal vector of SO(3) or SE(3)
- Returns
normal vector
- Return type
numpy.ndarray, shape=(3,)
Is the first column of the rotation submatrix, sometimes called the normal vector. Parallel to the x-axis of the frame defined by this pose.
-
property
o
¶ Orientation vector of SO(3) or SE(3)
- Returns
orientation vector
- Return type
numpy.ndarray, shape=(3,)
Is the second column of the rotation submatrix, sometimes called the orientation vector. Parallel to the y-axis of the frame defined by this pose.
-
property
a
¶ Approach vector of SO(3) or SE(3)
- Returns
approach vector
- Return type
numpy.ndarray, shape=(3,)
Is the third column of the rotation submatrix, sometimes called the approach vector. Parallel to the z-axis of the frame defined by this pose.
-
inv
()[source]¶ Inverse of SO(3)
- Parameters
self (SE3 instance) – pose
- Returns
inverse
- Return type
Returns the inverse, which for elements of SO(3) is the transpose.
-
eul
(unit='deg')[source]¶ SO(3) or SE(3) as Euler angles
- Parameters
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3-vector of Euler angles
- Return type
numpy.ndarray, shape=(3,)
x.eul
is the Euler angle representation of the rotation. Euler angles are a 3-vector \((\phi, heta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.If len(x) is:
1, return an ndarray with shape=(3,)
N>1, return ndarray with shape=(N,3)
ndarray with shape=(3,), if len(R) == 1
ndarray with shape=(N,3), if len(R) = N > 1
- Seealso
-
rpy
(unit='deg', order='zyx')[source]¶ SO(3) or SE(3) as roll-pitch-yaw angles
- Parameters
order (str) – angle sequence order, default to ‘zyx’
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3-vector of roll-pitch-yaw angles
- Return type
numpy.ndarray, shape=(3,)
x.rpy
is the roll-pitch-yaw angle representation of the rotation. The angles are a 3-vector \((r, p, y)\) which correspond to successive rotations about the axes specified byorder
:‘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. Covention 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 len(x) is:
1, return an ndarray with shape=(3,)
N>1, return ndarray with shape=(N,3)
- Seealso
-
Ad
()[source]¶ Adjoint of SO(3)
- Returns
adjoint matrix
- Return type
numpy.ndarray, shape=(6,6)
SE3.Ad
is the 6x6 adjoint matrix
- Seealso
Twist.ad.
-
static
isvalid
(x)[source]¶ Test if matrix is valid SO(3)
- Parameters
x (numpy.ndarray) – matrix to test
- Returns
true if the matrix is a valid element of SO(3), ie. it is a 3x3 orthonormal matrix with determinant of +1.
- Return type
bool
- Seealso
isrot()
-
classmethod
Rx
(theta, unit='rad')[source]¶ Construct a new SO(3) from X-axis rotation
- Parameters
theta (float or array_like) – rotation angle about the X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
SO(3) rotation
- Return type
SO3 instance
SE3.Rx(theta)
is an SO(3) rotation oftheta
radians about the x-axisSE3.Rx(theta, "deg")
as above buttheta
is in degrees
If
theta
is an array then the result is a sequence of rotations defined by consecutive elements.Example:
>>> x = SO3.Rx(np.linspace(0, math.pi, 20)) >>> len(x) 20 >>> x[7] SO3(array([[ 1. , 0. , 0. ], [ 0. , 0.40169542, -0.91577333], [ 0. , 0.91577333, 0.40169542]]))
-
classmethod
Ry
(theta, unit='rad')[source]¶ Construct a new SO(3) from Y-axis rotation
- Parameters
theta (float or array_like) – rotation angle about Y-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
SO(3) rotation
- Return type
SO3 instance
SO3.Ry(theta)
is an SO(3) rotation oftheta
radians about the y-axisSO3.Ry(theta, "deg")
as above buttheta
is in degrees
If
theta
is an array then the result is a sequence of rotations defined by consecutive elements.Example:
>>> x = SO3.Ry(np.linspace(0, math.pi, 20)) >>> len(x) 20 >>> x[7] >>> x[7] SO3(array([[ 0.40169542, 0. , 0.91577333], [ 0. , 1. , 0. ], [-0.91577333, 0. , 0.40169542]]))
-
classmethod
Rz
(theta, unit='rad')[source]¶ Construct a new SO(3) from Z-axis rotation
- Parameters
theta (float or array_like) – rotation angle about Z-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
SO(3) rotation
- Return type
SO3 instance
SO3.Rz(theta)
is an SO(3) rotation oftheta
radians about the z-axisSO3.Rz(theta, "deg")
as above buttheta
is in degrees
If
theta
is an array then the result is a sequence of rotations defined by consecutive elements.Example:
>>> x = SE3.Rz(np.linspace(0, math.pi, 20)) >>> len(x) 20 SO3(array([[ 0.40169542, -0.91577333, 0. ], [ 0.91577333, 0.40169542, 0. ], [ 0. , 0. , 1. ]]))
-
classmethod
Rand
(N=1)[source]¶ Construct a new SO(3) from random rotation
- Parameters
N (int) – number of random rotations
- Returns
SO(3) rotation matrix
- Return type
SO3 instance
SO3.Rand()
is a random SO(3) rotation.SO3.Rand(N)
is a sequence of N random rotations.
Example:
>>> x = SO3.Rand() >>> x SO3(array([[ 0.1805082 , -0.97959019, 0.08842995], [-0.98357187, -0.17961408, 0.01803234], [-0.00178104, -0.0902322 , -0.99591916]]))
-
classmethod
Eul
(angles, *, unit='rad')[source]¶ Construct a new SO(3) from Euler angles
- Parameters
angles (array_like or numpy.ndarray with shape=(N,3)) – Euler angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
SO(3) rotation
- Return type
SO3 instance
SO3.Eul(angles)
is an SO(3) rotation defined by a 3-vector of Euler angles \((\phi, \theta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.If
angles
is an Nx3 matrix then the result is a sequence of rotations each defined by Euler angles correponding to the rows ofangles
.- Seealso
-
classmethod
RPY
(angles, *, order='zyx', unit='rad')[source]¶ Construct a new SO(3) from roll-pitch-yaw angles
- Parameters
angles (array_like or numpy.ndarray with shape=(N,3)) – roll-pitch-yaw angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns
SO(3) rotation
- Return type
SO3 instance
SO3.RPY(angles)
is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles \((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. Covention 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 correponding to the rows of angles.- Seealso
-
classmethod
OA
(o, a)[source]¶ Construct a new SO(3) from two vectors
- Parameters
o (array_like) – 3-vector parallel to Y- axis
a – 3-vector parallel to the Z-axis
- Returns
SO(3) rotation
- Return type
SO3 instance
SO3.OA(O, A)
is an SO(3) rotation defined in terms of vectors parallel to the Y- and Z-axes of its reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N, O, A] and N = O x A.Notes:
Only the
A
vector is guaranteed to have the same direction in the resulting rotation matrixO
andA
do not have to be unit-length, they are normalizedO
and ``A` do not have to be orthogonal, so long as they are not parallel
-
classmethod
AngVec
(theta, v, *, unit='rad')[source]¶ Construct a new SO(3) rotation matrix from rotation angle and axis
- Parameters
theta (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (array_like) – rotation axis, 3-vector
- Returns
SO(3) rotation
- Return type
SO3 instance
SO3.AngVec(theta, V)
is an SO(3) rotation defined by a rotation ofTHETA
about the vectorV
.If \(\theta \eq 0\) the result in an identity matrix, otherwise
V
must have a finite length, ie. \(|V| > 0\).- Seealso
angvec()
,spatialmath.base.transforms3d.angvec2r()
-
classmethod
Exp
(S, check=True, so3=True)[source]¶ Create an SO(3) rotation matrix from so(3)
- Parameters
S (numpy ndarray) – Lie algebra so(3)
check (bool) – check that passed matrix is valid so(3), default True
so3 (bool) – input is an so(3) matrix (default True)
- Returns
SO(3) rotation
- Return type
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:` heta 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.
-
property
A
¶ Interal array representation (superclass property)
- Parameters
- Returns
The numeric array
- Return type
numpy.ndarray
Each pose subclass SO(N) or SE(N) are stored internally as a numpy array. This property returns the array, shape depends on the particular subclass.
Examples:
>>> x = SE3() >>> x.A array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
- Seealso
shape, N
-
classmethod
Empty
()¶ Construct a new pose object with zero items (superclass method)
- Parameters
- Returns
a pose with zero values
- Return type
This constructs an empty pose container which can be appended to. For example:
>>> x = SO2.Empty() >>> len(x) 0 >>> x.append(SO2(20, 'deg')) >>> len(x) 1
-
property
N
¶ Dimension of the object’s group (superclass property)
- Returns
dimension
- Return type
int
Dimension of the group is 2 for
SO2
orSE2
, and 3 forSO3
orSE3
. This corresponds to the dimension of the space, 2D or 3D, to which these rotations or rigid-body motions apply.Example:
>>> x = SE3() >>> x.N 3
-
__add__
(right)¶ Overloaded
+
operator (superclass method)- Parameters
left – left addend
right – right addend
- Returns
sum
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray, shape=(N,N)
Add elements of two poses. This is not a group operation so the result is a matrix not a pose class.
X + Y
is the element-wise sum of the matrix value ofX
andY
X + s
is the element-wise sum of the matrix value ofX
ands
s + X
is the element-wise sum of the matrix value ofs
andX
Operands
Sum
left
right
type
operation
Pose
Pose
NxN matrix
element-wise matrix sum
Pose
scalar
NxN matrix
element-wise sum
scalar
Pose
NxN matrix
element-wise sum
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar + Pose is handled by
__radd__
scalar addition is commutative
Any other input combinations result in a ValueError.
For pose addition the
left
andright
operands may be a sequence which results in the result being a sequence:len(left)
len(right)
len
operation
1
1
1
prod = left + right
1
M
M
prod[i] = left + right[i]
N
1
M
prod[i] = left[i] + right
M
M
M
prod[i] = left[i] + right[i]
-
__eq__
(right)¶ Overloaded
==
operator (superclass method)- Parameters
left – left side of comparison
right – right side of comparison
- Returns
poses are equal
- Return type
bool
Test two poses for equality
X == Y
is true of the poses are of the same type and numerically equal.
If either operand contains a sequence the results is a sequence according to:
len(left)
len(right)
len
operation
1
1
1
ret = left == right
1
M
M
ret[i] = left == right[i]
N
1
M
ret[i] = left[i] == right
M
M
M
ret[i] = left[i] == right[i]
-
__mul__
(right)¶ Overloaded
*
operator (superclass method)- Parameters
left – left multiplicand
right – right multiplicand
- Returns
product
- Raises
ValueError
Pose composition, scaling or vector transformation:
X * Y
compounds the posesX
andY
X * s
performs elementwise multiplication of the elements ofX
bys
s * X
performs elementwise multiplication of the elements ofX
bys
X * v
linear transform of the vectorv
Multiplicands
Product
left
right
type
operation
Pose
Pose
Pose
matrix product
Pose
scalar
NxN matrix
element-wise product
scalar
Pose
NxN matrix
element-wise product
Pose
N-vector
N-vector
vector transform
Pose
NxM matrix
NxM matrix
transform each column
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar x Pose is handled by
__rmul__
scalar multiplication is commutative but the result is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.
For pose composition the
left
andright
operands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right
1
M
M
prod[i] = left * right[i]
N
1
M
prod[i] = left[i] * right
M
M
M
prod[i] = left[i] * right[i]
For vector transformation there are three cases
Multiplicands
Product
len(left)
right.shape
shape
operation
1
(N,)
(N,)
vector transformation
M
(N,)
(N,M)
vector transformations
1
(N,M)
(N,M)
column transformation
Notes:
for the
SE2
andSE3
case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.
-
__ne__
(right)¶ Overloaded
!=
operator- Parameters
left – left side of comparison
right – right side of comparison
- Returns
poses are not equal
- Return type
bool
Test two poses for inequality
X == Y
is true of the poses are of the same type but not numerically equal.
If either operand contains a sequence the results is a sequence according to:
len(left)
len(right)
len
operation
1
1
1
ret = left != right
1
M
M
ret[i] = left != right[i]
N
1
M
ret[i] = left[i] != right
M
M
M
ret[i] = left[i] != right[i]
-
__pow__
(n)¶ Overloaded
**
operator (superclass method)- Parameters
n – pose
- Returns
pose to the power n
Raise all elements of pose to the specified power.
X**n
raise all values inX
to the powern
-
__sub__
(right)¶ Overloaded
-
operator (superclass method)- Parameters
left – left minuend
right – right subtrahend
- Returns
difference
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray, shape=(N,N)
Subtract elements of two poses. This is not a group operation so the result is a matrix not a pose class.
X - Y
is the element-wise difference of the matrix value ofX
andY
X - s
is the element-wise difference of the matrix value ofX
ands
s - X
is the element-wise difference ofs
and the matrix value ofX
Operands
Sum
left
right
type
operation
Pose
Pose
NxN matrix
element-wise matrix difference
Pose
scalar
NxN matrix
element-wise sum
scalar
Pose
NxN matrix
element-wise sum
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar - Pose is handled by
__rsub__
Any other input combinations result in a ValueError.
For pose addition the
left
andright
operands may be a sequence which results in the result being a sequence:len(left)
len(right)
len
operation
1
1
1
prod = left - right
1
M
M
prod[i] = left - right[i]
N
1
M
prod[i] = left[i] - right
M
M
M
prod[i] = left[i] right[i]
-
__truediv__
(right)¶ Overloaded
/
operator (superclass method)- Parameters
left – left multiplicand
right – right multiplicand
- Returns
product
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray
Pose composition or scaling:
X / Y
compounds the posesX
andY.inv()
X / s
performs elementwise multiplication of the elements ofX
bys
Multiplicands
Quotient
left
right
type
operation
Pose
Pose
Pose
matrix product by inverse
Pose
scalar
NxN matrix
element-wise division
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar multiplication is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.
For pose composition the
left
andright
operands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right.inv()
1
M
M
prod[i] = left * right[i].inv()
N
1
M
prod[i] = left[i] * right.inv()
M
M
M
prod[i] = left[i] * right[i].inv()
-
property
about
¶ Succinct summary of object type and length (superclass property)
- Returns
succinct summary
- Return type
str
Displays the type and the number of elements in compact form, for example:
>>> x = SE3([SE3() for i in range(20)]) >>> len(x) 20 >>> print(x.about) SE3[20]
-
animate
(*args, T0=None, **kwargs)¶ Plot pose object as an animated coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
as a coordinate frame moving from the origin, orT0
, in either 2D or 3D axes. There are many options, see the links below.
Example:
>>> X = SE3.Rx(0.3) >>> X.animate(frame='A', color='green')
- Seealso
-
append
(x)¶ Append a value to a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of appended object
Appends the argument to the object’s internal list of values.
Examples:
>>> x = SE3() >>> len(x) 1 >>> x.append(SE3.Rx(0.1)) >>> len(x) 2
-
clear
() → None -- remove all items from S¶
-
extend
(x)¶ Extend sequence of values of a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of appended object
Appends the argument to the object’s internal list of values.
Examples:
>>> x = SE3() >>> len(x) 1 >>> x.append(SE3.Rx(0.1)) >>> len(x) 2
-
insert
(i, value)¶ Insert a value to a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of inserted value
Inserts the argument into the object’s internal list of values.
Examples:
>>> x = SE3() >>> x.inert(0, SE3.Rx(0.1)) # insert at position 0 in the list >>> len(x) 2
-
interp
(s=None, T0=None)¶ Interpolate pose (superclass method)
- Parameters
- Returns
interpolated pose
- Return type
X.interp(s)
interpolates the pose X between identity when s=0 and X when s=1.
len(X)
len(s)
len(result)
Result
1
1
1
Y = interp(identity, X, s)
M
1
M
Y[i] = interp(T0, X[i], s)
1
M
M
Y[i] = interp(T0, X, s[i])
Example:
>>> x = SE3.Rx(0.3) >>> print(x.interp(0)) SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])) >>> print(x.interp(1)) SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.95533649, -0.29552021, 0. ], [ 0. , 0.29552021, 0.95533649, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> y = x.interp(x, np.linspace(0, 1, 10)) >>> len(y) 10 >>> y[5] SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.98614323, -0.16589613, 0. ], [ 0. , 0.16589613, 0.98614323, 0. ], [ 0. , 0. , 0. , 1. ]]))
Notes:
For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
-
property
isSE
¶ Test if object belongs to SE(n) group (superclass property)
-
property
isSO
¶ Test if object belongs to SO(n) group (superclass property)
-
ishom
()¶ Test if object belongs to SE(3) group (superclass method)
- Returns
True
if object is instance of SE3- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SE3)
.Example:
>>> x = SO3() >>> x.isrot() False >>> x = SE3() >>> x.isrot() True
-
ishom2
()¶ Test if object belongs to SE(2) group (superclass method)
- Returns
True
if object is instance of SE2- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SE2)
.Example:
>>> x = SO2() >>> x.isrot() False >>> x = SE2() >>> x.isrot() True
-
isrot
()¶ Test if object belongs to SO(3) group (superclass method)
- Returns
True
if object is instance of SO3- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SO3)
.Example:
>>> x = SO3() >>> x.isrot() True >>> x = SE3() >>> x.isrot() False
-
isrot2
()¶ Test if object belongs to SO(2) group (superclass method)
- Returns
True
if object is instance of SO2- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SO2)
.Example:
>>> x = SO2() >>> x.isrot() True >>> x = SE2() >>> x.isrot() False
-
log
()¶ Logarithm of pose (superclass method)
- Returns
logarithm
- Return type
numpy.ndarray
- Raises
ValueError
An efficient closed-form solution of the matrix logarithm.
Input
Output
Pose
Shape
Structure
SO2
(2,2)
skew-symmetric
SE2
(3,3)
augmented skew-symmetric
SO3
(3,3)
skew-symmetric
SE3
(4,4)
augmented skew-symmetric
Example:
>>> x = SE3.Rx(0.3) >>> y = x.log() >>> y array([[ 0. , -0. , 0. , 0. ], [ 0. , 0. , -0.3, 0. ], [-0. , 0.3, 0. , 0. ], [ 0. , 0. , 0. , 0. ]])
-
norm
()¶ Normalize pose (superclass method)
X.norm()
is an equivalent pose object but the rotational matrix part of all values has been adjusted to ensure it is a proper orthogonal matrix rotation.
Example:
>>> x = SE3() >>> y = x.norm() >>> y SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]))
Notes:
Only the direction of A vector (the z-axis) is unchanged.
Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.
- Seealso
trnorm()
,trnorm2()
-
plot
(*args, **kwargs)¶ Plot pose object as a coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
as a coordinate frame in either 2D or 3D axes. There are many options, see the links below.
Example:
>>> X = SE3.Rx(0.3) >>> X.plot(frame='A', color='green')
-
pop
()¶ Pop value of a pose object (superclass method)
- Returns
the specific element of the pose
- Return type
- Raises
IndexError – if there are no values to pop
Removes the first pose value from the sequence in the pose object.
Example:
>>> x = SE3.Rx([0, math.pi/2, math.pi]) >>> len(x) 3 >>> y = x.pop() >>> y SE3(array([[ 1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00], [ 0.0000000e+00, -1.0000000e+00, -1.2246468e-16, 0.0000000e+00], [ 0.0000000e+00, 1.2246468e-16, -1.0000000e+00, 0.0000000e+00], [ 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00]])) >>> len(x) 2
-
printline
(**kwargs)¶ Print pose as a single line (superclass method)
- Parameters
label (str) – text label to put at start of line
file (str) – file to write formatted string to. [default, stdout]
fmt (str) – conversion format for each number as used by
format()
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
optional formatted string
- Return type
str
For SO(3) or SE(3) also:
- Parameters
orient (str) – 3-angle convention to use
X.printline()
printX
in single-line format tostdout
, followed by a newlineX.printline(file=None)
return a string containingX
in single-line format
Example:
>>> x=SE3.Rx(0.3) >>> x.printline() t = 0, 0, 0; rpy/zyx = 17, 0, 0 deg
-
reverse
()¶ S.reverse() – reverse IN PLACE
-
property
shape
¶ Shape of the object’s matrix representation (superclass property)
- Returns
matrix shape
- Return type
2-tuple of ints
(2,2) for
SO2
, (3,3) forSE2
andSO3
, and (4,4) forSE3
.Example:
>>> x = SE3() >>> x.shape (4, 4)
-
-
class
spatialmath.pose3d.
SE3
(x=None, y=None, z=None, *, check=True)[source]¶ Bases:
spatialmath.pose3d.SO3
-
__init__
(x=None, y=None, z=None, *, check=True)[source]¶ Construct new SE(3) object
- Parameters
x (float) – translation distance along the X-axis
y (float) – translation distance along the Y-axis
z (float) – translation distance along the Z-axis
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3()
is a null motion – the identity matrixSE3(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. Ifcheck
isTrue
check the matrix belongs to SE(3).SE3([T1, T2, ... TN])
where each Ti is a 4x4 numpy array representing an SE(3) matrix, is an SE3 instance containing N rotations. Ifcheck
isTrue
check the matrix belongs to SE(3).SE3([X1, X2, ... XN])
where each Xi is an SE3 instance, is an SE3 instance containing N rotations.
-
property
t
¶ Translational component of SE(3)
- Parameters
self (SE3 instance) – SE(3)
- Returns
translational component
- Return type
numpy.ndarray
T.t
returns an:ndarray with shape=(3,), if len(T) == 1
ndarray with shape=(N,3), if len(T) = N > 1
-
inv
()[source]¶ Inverse of SE(3)
- Returns
inverse
- Return type
Returns the inverse taking into account its structure
\(T = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]\)
- Seealso
trinv()
-
delta
(X2)[source]¶ Difference of SE(3)
- Parameters
X1 (SE3) –
- Returns
differential motion vector
- Return type
numpy.ndarray, shape=(6,)
X1.delta(T2)
is the differential motion (6x1) corresponding to infinitessimal motion (in the X1 frame) from pose X1 to X2.
The vector \(d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z\) represents infinitessimal translation and rotation.
Notes:
the displacement is only an approximation to the motion T, and assumes that X1 ~ X2.
Can be considered as an approximation to the effect of spatial velocity over a a time interval, average spatial velocity multiplied by time.
Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
- Seealso
tr2delta()
-
static
isvalid
(x)[source]¶ Test if matrix is valid SE(3)
- Parameters
x (numpy.ndarray) – matrix to test
- Returns
true of the matrix is 4x4 and a valid element of SE(3), ie. it is an homogeneous transformation matrix.
- Return type
bool
- Seealso
ishom()
-
classmethod
Rx
(theta, unit='rad')[source]¶ Create SE(3) pure rotation about the X-axis
- Parameters
theta (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.Rx(THETA)
is an SO(3) rotation of THETA radians about the x-axisSE3.Rx(THETA, "deg")
as above but THETA is in degrees
If
theta
is an array then the result is a sequence of rotations defined by consecutive elements.
-
classmethod
Ry
(theta, unit='rad')[source]¶ Create SE(3) pure rotation about the Y-axis
- Parameters
theta (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.Ry(THETA)
is an SO(3) rotation of THETA radians about the y-axisSE3.Ry(THETA, "deg")
as above but THETA is in degrees
If
theta
is an array then the result is a sequence of rotations defined by consecutive elements.
-
classmethod
Rz
(theta, unit='rad')[source]¶ Create SE(3) pure rotation about the Z-axis
- Parameters
theta (float) – rotation angle about Z-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.Rz(THETA)
is an SO(3) rotation of THETA radians about the z-axisSE3.Rz(THETA, "deg")
as above but THETA is in degrees
If
theta
is an array then the result is a sequence of rotations defined by consecutive elements.
-
classmethod
Rand
(*, xrange=[-1, 1], yrange=[-1, 1], zrange=[-1, 1], N=1)[source]¶ Create a random SE(3)
- Parameters
xrange (2-element sequence, optional) – x-axis range [min,max], defaults to [-1, 1]
yrange (2-element sequence, optional) – y-axis range [min,max], defaults to [-1, 1]
zrange (2-element sequence, optional) – z-axis range [min,max], defaults to [-1, 1]
N (int) – number of random transforms
- Returns
homogeneous transformation matrix
- Return type
SE3 instance
Return an SE3 instance with random rotation and translation.
SE3.Rand()
is a random SE(3) translation.SE3.Rand(N)
is an SE3 object containing a sequence of N random poses.
- Seealso
~spatialmath.quaternion.UnitQuaternion.Rand
-
classmethod
Eul
(angles, unit='rad')[source]¶ Create an SE(3) pure rotation from Euler angles
- Parameters
angles (array_like or numpy.ndarray with shape=(N,3)) – 3-vector of Euler angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.Eul(ANGLES)
is an SO(3) rotation defined by a 3-vector of Euler angles \((\phi, heta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.If
angles
is an Nx3 matrix then the result is a sequence of rotations each defined by Euler angles correponding to the rows of angles.- Seealso
-
classmethod
RPY
(angles, order='zyx', unit='rad')[source]¶ Create an SO(3) pure rotation from roll-pitch-yaw angles
- Parameters
angles (array_like or numpy.ndarray with shape=(N,3)) – 3-vector of roll-pitch-yaw angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.RPY(ANGLES)
is an SE(3) rotation defined by a 3-vector of roll, pitch, yaw angles \((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. Covention 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 correponding to the rows of angles.- Seealso
-
classmethod
OA
(o, a)[source]¶ Create SE(3) pure rotation from two vectors
- Parameters
o (array_like) – 3-vector parallel to Y- axis
a – 3-vector parallel to the Z-axis
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.OA(O, A)
is an SE(3) rotation defined in terms of vectors parallel to the Y- and Z-axes of its reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N O A] and N = O x A.Notes:
The A vector is the only guaranteed to have the same direction in the resulting rotation matrix
O and A do not have to be unit-length, they are normalized
O and A do not have to be orthogonal, so long as they are not parallel
The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.
-
classmethod
AngVec
(theta, v, *, unit='rad')[source]¶ Create an SE(3) pure rotation matrix from rotation angle and axis
- Parameters
theta (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (array_like) – rotation axis, 3-vector
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.AngVec(THETA, V)
is an SE(3) rotation defined by a rotation ofTHETA
about the vectorV
.Notes:
If
THETA == 0
then return identity matrix.If
THETA ~= 0
thenV
must have a finite length.
- Seealso
angvec()
,spatialmath.base.transforms3d.angvec2r()
-
classmethod
Exp
(S)[source]¶ Create an SE(3) rotation matrix from se(3)
- Parameters
S (numpy ndarray) – Lie algebra se(3)
- Returns
3x3 rotation matrix
- Return type
SO3 instance
SE3.Exp(S)
is an SE(3) rotation defined by its Lie algebra which is a 3x3 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)
-
classmethod
Tx
(x)[source]¶ Create SE(3) translation along the X-axis
- Parameters
theta (float) – translation distance along the X-axis
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.Tz(D)` is an SE(3) translation of D along the x-axis
-
classmethod
Ty
(y)[source]¶ Create SE(3) translation along the Y-axis
- Parameters
theta (float) – translation distance along the Y-axis
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.Tz(D)` is an SE(3) translation of D along the y-axis
-
classmethod
Tz
(z)[source]¶ Create SE(3) translation along the Z-axis
- Parameters
theta (float) – translation distance along the Z-axis
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
SE3.Tz(D)` is an SE(3) translation of D along the z-axis
-
property
A
¶ Interal array representation (superclass property)
- Parameters
- Returns
The numeric array
- Return type
numpy.ndarray
Each pose subclass SO(N) or SE(N) are stored internally as a numpy array. This property returns the array, shape depends on the particular subclass.
Examples:
>>> x = SE3() >>> x.A array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
- Seealso
shape, N
-
Ad
()¶ Adjoint of SO(3)
- Returns
adjoint matrix
- Return type
numpy.ndarray, shape=(6,6)
SE3.Ad
is the 6x6 adjoint matrix
- Seealso
Twist.ad.
-
classmethod
Delta
(d)[source]¶ Create SE(3) from diffential motion
- Parameters
d (6-element array_like) – differential motion
- Returns
4x4 homogeneous transformation matrix
- Return type
SE3 instance
T = delta2tr(d)
is an SE(3) representing differential motion \(d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z\).
Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
- Seealso
delta()
,delta2tr()
-
classmethod
Empty
()¶ Construct a new pose object with zero items (superclass method)
- Parameters
- Returns
a pose with zero values
- Return type
This constructs an empty pose container which can be appended to. For example:
>>> x = SO2.Empty() >>> len(x) 0 >>> x.append(SO2(20, 'deg')) >>> len(x) 1
-
property
N
¶ Dimension of the object’s group (superclass property)
- Returns
dimension
- Return type
int
Dimension of the group is 2 for
SO2
orSE2
, and 3 forSO3
orSE3
. This corresponds to the dimension of the space, 2D or 3D, to which these rotations or rigid-body motions apply.Example:
>>> x = SE3() >>> x.N 3
-
property
R
¶ SO(3) or SE(3) as rotation matrix
- Returns
rotational component
- Return type
numpy.ndarray, shape=(3,3)
x.R
returns the rotation matrix, when x is SO3 or SE3. If len(x) is:1, return an ndarray with shape=(3,3)
N>1, return ndarray with shape=(N,3,3)
-
__add__
(right)¶ Overloaded
+
operator (superclass method)- Parameters
left – left addend
right – right addend
- Returns
sum
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray, shape=(N,N)
Add elements of two poses. This is not a group operation so the result is a matrix not a pose class.
X + Y
is the element-wise sum of the matrix value ofX
andY
X + s
is the element-wise sum of the matrix value ofX
ands
s + X
is the element-wise sum of the matrix value ofs
andX
Operands
Sum
left
right
type
operation
Pose
Pose
NxN matrix
element-wise matrix sum
Pose
scalar
NxN matrix
element-wise sum
scalar
Pose
NxN matrix
element-wise sum
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar + Pose is handled by
__radd__
scalar addition is commutative
Any other input combinations result in a ValueError.
For pose addition the
left
andright
operands may be a sequence which results in the result being a sequence:len(left)
len(right)
len
operation
1
1
1
prod = left + right
1
M
M
prod[i] = left + right[i]
N
1
M
prod[i] = left[i] + right
M
M
M
prod[i] = left[i] + right[i]
-
__eq__
(right)¶ Overloaded
==
operator (superclass method)- Parameters
left – left side of comparison
right – right side of comparison
- Returns
poses are equal
- Return type
bool
Test two poses for equality
X == Y
is true of the poses are of the same type and numerically equal.
If either operand contains a sequence the results is a sequence according to:
len(left)
len(right)
len
operation
1
1
1
ret = left == right
1
M
M
ret[i] = left == right[i]
N
1
M
ret[i] = left[i] == right
M
M
M
ret[i] = left[i] == right[i]
-
__mul__
(right)¶ Overloaded
*
operator (superclass method)- Parameters
left – left multiplicand
right – right multiplicand
- Returns
product
- Raises
ValueError
Pose composition, scaling or vector transformation:
X * Y
compounds the posesX
andY
X * s
performs elementwise multiplication of the elements ofX
bys
s * X
performs elementwise multiplication of the elements ofX
bys
X * v
linear transform of the vectorv
Multiplicands
Product
left
right
type
operation
Pose
Pose
Pose
matrix product
Pose
scalar
NxN matrix
element-wise product
scalar
Pose
NxN matrix
element-wise product
Pose
N-vector
N-vector
vector transform
Pose
NxM matrix
NxM matrix
transform each column
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar x Pose is handled by
__rmul__
scalar multiplication is commutative but the result is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.
For pose composition the
left
andright
operands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right
1
M
M
prod[i] = left * right[i]
N
1
M
prod[i] = left[i] * right
M
M
M
prod[i] = left[i] * right[i]
For vector transformation there are three cases
Multiplicands
Product
len(left)
right.shape
shape
operation
1
(N,)
(N,)
vector transformation
M
(N,)
(N,M)
vector transformations
1
(N,M)
(N,M)
column transformation
Notes:
for the
SE2
andSE3
case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.
-
__ne__
(right)¶ Overloaded
!=
operator- Parameters
left – left side of comparison
right – right side of comparison
- Returns
poses are not equal
- Return type
bool
Test two poses for inequality
X == Y
is true of the poses are of the same type but not numerically equal.
If either operand contains a sequence the results is a sequence according to:
len(left)
len(right)
len
operation
1
1
1
ret = left != right
1
M
M
ret[i] = left != right[i]
N
1
M
ret[i] = left[i] != right
M
M
M
ret[i] = left[i] != right[i]
-
__pow__
(n)¶ Overloaded
**
operator (superclass method)- Parameters
n – pose
- Returns
pose to the power n
Raise all elements of pose to the specified power.
X**n
raise all values inX
to the powern
-
__sub__
(right)¶ Overloaded
-
operator (superclass method)- Parameters
left – left minuend
right – right subtrahend
- Returns
difference
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray, shape=(N,N)
Subtract elements of two poses. This is not a group operation so the result is a matrix not a pose class.
X - Y
is the element-wise difference of the matrix value ofX
andY
X - s
is the element-wise difference of the matrix value ofX
ands
s - X
is the element-wise difference ofs
and the matrix value ofX
Operands
Sum
left
right
type
operation
Pose
Pose
NxN matrix
element-wise matrix difference
Pose
scalar
NxN matrix
element-wise sum
scalar
Pose
NxN matrix
element-wise sum
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar - Pose is handled by
__rsub__
Any other input combinations result in a ValueError.
For pose addition the
left
andright
operands may be a sequence which results in the result being a sequence:len(left)
len(right)
len
operation
1
1
1
prod = left - right
1
M
M
prod[i] = left - right[i]
N
1
M
prod[i] = left[i] - right
M
M
M
prod[i] = left[i] right[i]
-
__truediv__
(right)¶ Overloaded
/
operator (superclass method)- Parameters
left – left multiplicand
right – right multiplicand
- Returns
product
- Raises
ValueError – for incompatible arguments
- Returns
matrix
- Return type
numpy ndarray
Pose composition or scaling:
X / Y
compounds the posesX
andY.inv()
X / s
performs elementwise multiplication of the elements ofX
bys
Multiplicands
Quotient
left
right
type
operation
Pose
Pose
Pose
matrix product by inverse
Pose
scalar
NxN matrix
element-wise division
Notes:
Pose is
SO2
,SE2
,SO3
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
scalar multiplication is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.
For pose composition the
left
andright
operands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right.inv()
1
M
M
prod[i] = left * right[i].inv()
N
1
M
prod[i] = left[i] * right.inv()
M
M
M
prod[i] = left[i] * right[i].inv()
-
property
a
¶ Approach vector of SO(3) or SE(3)
- Returns
approach vector
- Return type
numpy.ndarray, shape=(3,)
Is the third column of the rotation submatrix, sometimes called the approach vector. Parallel to the z-axis of the frame defined by this pose.
-
property
about
¶ Succinct summary of object type and length (superclass property)
- Returns
succinct summary
- Return type
str
Displays the type and the number of elements in compact form, for example:
>>> x = SE3([SE3() for i in range(20)]) >>> len(x) 20 >>> print(x.about) SE3[20]
-
animate
(*args, T0=None, **kwargs)¶ Plot pose object as an animated coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
as a coordinate frame moving from the origin, orT0
, in either 2D or 3D axes. There are many options, see the links below.
Example:
>>> X = SE3.Rx(0.3) >>> X.animate(frame='A', color='green')
- Seealso
-
append
(x)¶ Append a value to a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of appended object
Appends the argument to the object’s internal list of values.
Examples:
>>> x = SE3() >>> len(x) 1 >>> x.append(SE3.Rx(0.1)) >>> len(x) 2
-
clear
() → None -- remove all items from S¶
-
eul
(unit='deg')¶ SO(3) or SE(3) as Euler angles
- Parameters
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3-vector of Euler angles
- Return type
numpy.ndarray, shape=(3,)
x.eul
is the Euler angle representation of the rotation. Euler angles are a 3-vector \((\phi, heta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.If len(x) is:
1, return an ndarray with shape=(3,)
N>1, return ndarray with shape=(N,3)
ndarray with shape=(3,), if len(R) == 1
ndarray with shape=(N,3), if len(R) = N > 1
- Seealso
-
extend
(x)¶ Extend sequence of values of a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of appended object
Appends the argument to the object’s internal list of values.
Examples:
>>> x = SE3() >>> len(x) 1 >>> x.append(SE3.Rx(0.1)) >>> len(x) 2
-
insert
(i, value)¶ Insert a value to a pose object (superclass method)
- Parameters
- Raises
ValueError – incorrect type of inserted value
Inserts the argument into the object’s internal list of values.
Examples:
>>> x = SE3() >>> x.inert(0, SE3.Rx(0.1)) # insert at position 0 in the list >>> len(x) 2
-
interp
(s=None, T0=None)¶ Interpolate pose (superclass method)
- Parameters
- Returns
interpolated pose
- Return type
X.interp(s)
interpolates the pose X between identity when s=0 and X when s=1.
len(X)
len(s)
len(result)
Result
1
1
1
Y = interp(identity, X, s)
M
1
M
Y[i] = interp(T0, X[i], s)
1
M
M
Y[i] = interp(T0, X, s[i])
Example:
>>> x = SE3.Rx(0.3) >>> print(x.interp(0)) SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])) >>> print(x.interp(1)) SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.95533649, -0.29552021, 0. ], [ 0. , 0.29552021, 0.95533649, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> y = x.interp(x, np.linspace(0, 1, 10)) >>> len(y) 10 >>> y[5] SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.98614323, -0.16589613, 0. ], [ 0. , 0.16589613, 0.98614323, 0. ], [ 0. , 0. , 0. , 1. ]]))
Notes:
For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
-
property
isSE
¶ Test if object belongs to SE(n) group (superclass property)
-
property
isSO
¶ Test if object belongs to SO(n) group (superclass property)
-
ishom
()¶ Test if object belongs to SE(3) group (superclass method)
- Returns
True
if object is instance of SE3- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SE3)
.Example:
>>> x = SO3() >>> x.isrot() False >>> x = SE3() >>> x.isrot() True
-
ishom2
()¶ Test if object belongs to SE(2) group (superclass method)
- Returns
True
if object is instance of SE2- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SE2)
.Example:
>>> x = SO2() >>> x.isrot() False >>> x = SE2() >>> x.isrot() True
-
isrot
()¶ Test if object belongs to SO(3) group (superclass method)
- Returns
True
if object is instance of SO3- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SO3)
.Example:
>>> x = SO3() >>> x.isrot() True >>> x = SE3() >>> x.isrot() False
-
isrot2
()¶ Test if object belongs to SO(2) group (superclass method)
- Returns
True
if object is instance of SO2- Return type
bool
For compatibility with Spatial Math Toolbox for MATLAB. In Python use
isinstance(x, SO2)
.Example:
>>> x = SO2() >>> x.isrot() True >>> x = SE2() >>> x.isrot() False
-
log
()¶ Logarithm of pose (superclass method)
- Returns
logarithm
- Return type
numpy.ndarray
- Raises
ValueError
An efficient closed-form solution of the matrix logarithm.
Input
Output
Pose
Shape
Structure
SO2
(2,2)
skew-symmetric
SE2
(3,3)
augmented skew-symmetric
SO3
(3,3)
skew-symmetric
SE3
(4,4)
augmented skew-symmetric
Example:
>>> x = SE3.Rx(0.3) >>> y = x.log() >>> y array([[ 0. , -0. , 0. , 0. ], [ 0. , 0. , -0.3, 0. ], [-0. , 0.3, 0. , 0. ], [ 0. , 0. , 0. , 0. ]])
-
property
n
¶ Normal vector of SO(3) or SE(3)
- Returns
normal vector
- Return type
numpy.ndarray, shape=(3,)
Is the first column of the rotation submatrix, sometimes called the normal vector. Parallel to the x-axis of the frame defined by this pose.
-
norm
()¶ Normalize pose (superclass method)
X.norm()
is an equivalent pose object but the rotational matrix part of all values has been adjusted to ensure it is a proper orthogonal matrix rotation.
Example:
>>> x = SE3() >>> y = x.norm() >>> y SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]))
Notes:
Only the direction of A vector (the z-axis) is unchanged.
Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.
- Seealso
trnorm()
,trnorm2()
-
property
o
¶ Orientation vector of SO(3) or SE(3)
- Returns
orientation vector
- Return type
numpy.ndarray, shape=(3,)
Is the second column of the rotation submatrix, sometimes called the orientation vector. Parallel to the y-axis of the frame defined by this pose.
-
plot
(*args, **kwargs)¶ Plot pose object as a coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
as a coordinate frame in either 2D or 3D axes. There are many options, see the links below.
Example:
>>> X = SE3.Rx(0.3) >>> X.plot(frame='A', color='green')
-
pop
()¶ Pop value of a pose object (superclass method)
- Returns
the specific element of the pose
- Return type
- Raises
IndexError – if there are no values to pop
Removes the first pose value from the sequence in the pose object.
Example:
>>> x = SE3.Rx([0, math.pi/2, math.pi]) >>> len(x) 3 >>> y = x.pop() >>> y SE3(array([[ 1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00], [ 0.0000000e+00, -1.0000000e+00, -1.2246468e-16, 0.0000000e+00], [ 0.0000000e+00, 1.2246468e-16, -1.0000000e+00, 0.0000000e+00], [ 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00]])) >>> len(x) 2
-
printline
(**kwargs)¶ Print pose as a single line (superclass method)
- Parameters
label (str) – text label to put at start of line
file (str) – file to write formatted string to. [default, stdout]
fmt (str) – conversion format for each number as used by
format()
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
optional formatted string
- Return type
str
For SO(3) or SE(3) also:
- Parameters
orient (str) – 3-angle convention to use
X.printline()
printX
in single-line format tostdout
, followed by a newlineX.printline(file=None)
return a string containingX
in single-line format
Example:
>>> x=SE3.Rx(0.3) >>> x.printline() t = 0, 0, 0; rpy/zyx = 17, 0, 0 deg
-
reverse
()¶ S.reverse() – reverse IN PLACE
-
rpy
(unit='deg', order='zyx')¶ SO(3) or SE(3) as roll-pitch-yaw angles
- Parameters
order (str) – angle sequence order, default to ‘zyx’
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3-vector of roll-pitch-yaw angles
- Return type
numpy.ndarray, shape=(3,)
x.rpy
is the roll-pitch-yaw angle representation of the rotation. The angles are a 3-vector \((r, p, y)\) which correspond to successive rotations about the axes specified byorder
:‘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. Covention 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 len(x) is:
1, return an ndarray with shape=(3,)
N>1, return ndarray with shape=(N,3)
- Seealso
-
property
shape
¶ Shape of the object’s matrix representation (superclass property)
- Returns
matrix shape
- Return type
2-tuple of ints
(2,2) for
SO2
, (3,3) forSE2
andSO3
, and (4,4) forSE3
.Example:
>>> x = SE3() >>> x.shape (4, 4)
-
-
class
spatialmath.quaternion.
Quaternion
(s=None, v=None, check=True, norm=True)[source]¶ Bases:
collections.UserList
A quaternion is a compact method of representing a 3D rotation that has computational advantages including speed and numerical robustness.
- A quaternion has 2 parts, a scalar s, and a 3-vector v and is typically written:
q = s <vx vy vz>
-
__init__
(s=None, v=None, check=True, norm=True)[source]¶ A zero quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}. A quaternion can be considered as a rotation about a vector in space where q = cos (theta/2) sin(theta/2) <vx vy vz> where <vx vy vz> is a unit vector. :param s: scalar :param v: vector
-
property
s
¶ - Parameters
q (Quaternion, UnitQuaternion) – input quaternion
- Returns
real part of quaternion
- Return type
float or numpy.ndarray
If the quaternion is of length one, a scalar float is returned.
If the quaternion is of length >1, a numpy array shape=(N,) is returned.
-
property
v
¶ - Parameters
q (Quaternion, UnitQuaternion) – input quaternion
- Returns
vector part of quaternion
- Return type
numpy ndarray
If the quaternion is of length one, a numpy array shape=(3,) is returned.
If the quaternion is of length >1, a numpy array shape=(N,3) is returned.
-
property
vec
¶ - Parameters
q (Quaternion, UnitQuaternion) – input quaternion
- Returns
quaternion expressed as a vector
- Return type
numpy ndarray
If the quaternion is of length one, a numpy array shape=(4,) is returned.
If the quaternion is of length >1, a numpy array shape=(N,4) is returned.
-
property
conj
¶
-
property
norm
¶ Return the norm of this quaternion. Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: number @return: the norm
-
property
unit
¶ Return an equivalent unit quaternion Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: quaternion @return: equivalent unit quaternion
-
property
matrix
¶
-
__mul__
(right)[source]¶ multiply quaternion
- Parameters
left (Quaternion, UnitQuaternion, float) – left multiplicand
right – right multiplicand
- Returns
product
- Return type
- Raises
ValueError
Multiplicands
Product
left
right
type
result
Quaternion
Quaternion
Quaternion
Hamilton product
Quaternion
UnitQuaternion
Quaternion
Hamilton product
Quaternion
scalar
Quaternion
scalar product
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
left
right
len
operation
1
1
1
prod = left * right
1
N
N
prod[i] = left * right[i]
N
1
N
prod[i] = left[i] * right
N
N
N
prod[i] = left[i] * right[i]
N
M
ValueError
-
__add__
(right)[source]¶ add quaternions
- Parameters
left (Quaternion, UnitQuaternion) – left addend
right (Quaternion, UnitQuaternion, float) – right addend
- Returns
sum
- Return type
- Raises
ValueError
Operands
Sum
left
right
type
result
Quaternion
Quaternion
Quaternion
elementwise sum
Quaternion
UnitQuaternion
Quaternion
elementwise sum
Quaternion
scalar
Quaternion
add to each element
UnitQuaternion
Quaternion
Quaternion
elementwise sum
UnitQuaternion
UnitQuaternion
Quaternion
elementwise sum
UnitQuaternion
scalar
Quaternion
add to each element
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
left
right
len
operation
1
1
1
prod = left + right
1
N
N
prod[i] = left + right[i]
N
1
N
prod[i] = left[i] + right
N
N
N
prod[i] = left[i] + right[i]
N
M
ValueError
A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
-
__sub__
(right)[source]¶ subtract quaternions
- Parameters
left (Quaternion, UnitQuaternion) – left minuend
right (Quaternion, UnitQuaternion, float) – right subtahend
- Returns
difference
- Return type
- Raises
ValueError
Operands
Difference
left
right
type
result
Quaternion
Quaternion
Quaternion
elementwise sum
Quaternion
UnitQuaternion
Quaternion
elementwise sum
Quaternion
scalar
Quaternion
subtract from each element
UnitQuaternion
Quaternion
Quaternion
elementwise sum
UnitQuaternion
UnitQuaternion
Quaternion
elementwise sum
UnitQuaternion
scalar
Quaternion
subtract from each element
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
left
right
len
operation
1
1
1
prod = left - right
1
N
N
prod[i] = left - right[i]
N
1
N
prod[i] = left[i] - right
N
N
N
prod[i] = left[i] - right[i]
N
M
ValueError
A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
-
clear
() → None -- remove all items from S¶
-
extend
(other)¶ S.extend(iterable) – extend sequence by appending elements from the iterable
-
insert
(i, item)¶ S.insert(index, value) – insert value before index
-
pop
([index]) → item -- remove and return item at index (default last).¶ Raise IndexError if list is empty or index is out of range.
-
reverse
()¶ S.reverse() – reverse IN PLACE
-
class
spatialmath.quaternion.
UnitQuaternion
(s=None, v=None, norm=True, check=True)[source]¶ Bases:
spatialmath.quaternion.Quaternion
A unit-quaternion is is a quaternion with unit length, that is \(s^2+v_x^2+v_y^2+v_z^2 = 1\).
A unit-quaternion can be considered as a rotation \(\theta\) where \(q = \cos \theta/2 \sin \theta/2 <v_x v_y v_z>\).
-
__init__
(s=None, v=None, norm=True, check=True)[source]¶ Construct a UnitQuaternion object
- Parameters
norm (bool) – explicitly normalize the quaternion [default True]
check (bool) – explicitly check dimension of passed lists [default True]
- Returns
new unit uaternion
- Return type
- Raises
ValueError
Single element quaternion:
UnitQuaternion()
constructs the identity quaternion 1<0,0,0>UnitQuaternion(s, v)
constructs a unit quaternion with specified reals
andv
vector parts.v
is a 3-vector given as a list, tuple, numpy.ndarrayUnitQuaternion(v)
constructs a unit quaternion with specified elements fromv
which is a 4-vector given as a list, tuple, numpy.ndarrayUnitQuaternion(R)
constructs a unit quaternion from an orthonormal rotation matrix given as a 3x3 numpy.ndarray. Ifcheck
is True test the matrix for orthogonality.
Multi-element quaternion:
UnitQuaternion(V)
constructs a unit quaternion list with specified elements fromV
which is an Nx4 numpy.ndarray, each row is a quaternion. Ifnorm
is True explicitly normalize each row.UnitQuaternion(L)
constructs a unit quaternion list from a list of 4-element numpy.ndarrays. Ifcheck
is True test each element of the list is a 4-vector. Ifnorm
is True explicitly normalize each vector.
-
property
R
¶
-
property
vec3
¶
-
classmethod
Rx
(angle, unit='rad')[source]¶ Construct a UnitQuaternion object representing rotation about X-axis
- Parameters
angle – rotation angle
unit (str) – rotation unit ‘rad’ [default] or ‘deg’
- Returns
new unit-quaternion
- Return type
UnitQuaternion(theta)
constructs a unit quaternion representing a rotation of theta radians about the X-axis.UnitQuaternion(theta, 'deg')
constructs a unit quaternion representing a rotation of theta degrees about the X-axis.
-
classmethod
Ry
(angle, unit='rad')[source]¶ Construct a UnitQuaternion object representing rotation about Y-axis
- Parameters
angle – rotation angle
unit (str) – rotation unit ‘rad’ [default] or ‘deg’
- Returns
new unit-quaternion
- Return type
UnitQuaternion(theta)
constructs a unit quaternion representing a rotation of theta radians about the Y-axis.UnitQuaternion(theta, 'deg')
constructs a unit quaternion representing a rotation of theta degrees about the Y-axis.
-
classmethod
Rz
(angle, unit='rad')[source]¶ Construct a UnitQuaternion object representing rotation about Z-axis
- Parameters
angle – rotation angle
unit (str) – rotation unit ‘rad’ [default] or ‘deg’
- Returns
new unit-quaternion
- Return type
UnitQuaternion(theta)
constructs a unit quaternion representing a rotation of theta radians about the Z-axis.UnitQuaternion(theta, 'deg')
constructs a unit quaternion representing a rotation of theta degrees about the Z-axis.
-
classmethod
Rand
(N=1)[source]¶ Create SO(3) with random rotation
- Parameters
N (int) – number of random rotations
- Returns
3x3 rotation matrix
- Return type
SO3 instance
SO3.Rand()
is a random SO(3) rotation.SO3.Rand(N)
is an SO3 object containing a sequence of N random rotations.
-
classmethod
Eul
(angles, *, unit='rad')[source]¶ Create an SO(3) rotation from Euler angles
- Parameters
angles (array_like) – 3-vector of Euler angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3x3 rotation matrix
- Return type
SO3 instance
SO3.Eul(ANGLES)
is an SO(3) rotation defined by a 3-vector of Euler angles \((\phi, heta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.- Seealso
-
classmethod
RPY
(angles, *, order='zyx', unit='rad')[source]¶ Create an SO(3) rotation from roll-pitch-yaw angles
- Parameters
angles (array_like) – 3-vector of roll-pitch-yaw angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns
3x3 rotation matrix
- Return type
SO3 instance
SO3.RPY(ANGLES)
is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles \((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. Covention 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.
- Seealso
-
classmethod
OA
(o, a)[source]¶ Create SO(3) rotation from two vectors
- Parameters
o (array_like) – 3-vector parallel to Y- axis
a – 3-vector parallel to the Z-axis
- Returns
3x3 rotation matrix
- Return type
SO3 instance
SO3.OA(O, A)
is an SO(3) rotation defined in terms of vectors parallel to the Y- and Z-axes of its reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N O A] and N = O x A.Notes:
The A vector is the only guaranteed to have the same direction in the resulting rotation matrix
O and A do not have to be unit-length, they are normalized
O and A do not have to be orthogonal, so long as they are not parallel
The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.
-
classmethod
AngVec
(theta, v, *, unit='rad')[source]¶ Create an SO(3) rotation matrix from rotation angle and axis
- Parameters
theta (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (array_like) – rotation axis, 3-vector
- Returns
3x3 rotation matrix
- Return type
SO3 instance
SO3.AngVec(THETA, V)
is an SO(3) rotation defined by a rotation ofTHETA
about the vectorV
.Notes:
If
THETA == 0
then return identity matrix.If
THETA ~= 0
thenV
must have a finite length.
- Seealso
angvec()
,spatialmath.base.transforms3d.angvec2r()
-
property
inv
¶
-
__add__
(right)¶ add quaternions
- Parameters
left (Quaternion, UnitQuaternion) – left addend
right (Quaternion, UnitQuaternion, float) – right addend
- Returns
sum
- Return type
- Raises
ValueError
Operands
Sum
left
right
type
result
Quaternion
Quaternion
Quaternion
elementwise sum
Quaternion
UnitQuaternion
Quaternion
elementwise sum
Quaternion
scalar
Quaternion
add to each element
UnitQuaternion
Quaternion
Quaternion
elementwise sum
UnitQuaternion
UnitQuaternion
Quaternion
elementwise sum
UnitQuaternion
scalar
Quaternion
add to each element
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
left
right
len
operation
1
1
1
prod = left + right
1
N
N
prod[i] = left + right[i]
N
1
N
prod[i] = left[i] + right
N
N
N
prod[i] = left[i] + right[i]
N
M
ValueError
A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
-
__sub__
(right)¶ subtract quaternions
- Parameters
left (Quaternion, UnitQuaternion) – left minuend
right (Quaternion, UnitQuaternion, float) – right subtahend
- Returns
difference
- Return type
- Raises
ValueError
Operands
Difference
left
right
type
result
Quaternion
Quaternion
Quaternion
elementwise sum
Quaternion
UnitQuaternion
Quaternion
elementwise sum
Quaternion
scalar
Quaternion
subtract from each element
UnitQuaternion
Quaternion
Quaternion
elementwise sum
UnitQuaternion
UnitQuaternion
Quaternion
elementwise sum
UnitQuaternion
scalar
Quaternion
subtract from each element
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
left
right
len
operation
1
1
1
prod = left - right
1
N
N
prod[i] = left - right[i]
N
1
N
prod[i] = left[i] - right
N
N
N
prod[i] = left[i] - right[i]
N
M
ValueError
A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
-
append
(x)¶ S.append(value) – append value to the end of the sequence
-
clear
() → None -- remove all items from S¶
-
property
conj
¶
-
extend
(other)¶ S.extend(iterable) – extend sequence by appending elements from the iterable
-
inner
(other)¶
-
insert
(i, item)¶ S.insert(index, value) – insert value before index
-
property
matrix
¶
-
property
norm
¶ Return the norm of this quaternion. Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: number @return: the norm
-
pop
([index]) → item -- remove and return item at index (default last).¶ Raise IndexError if list is empty or index is out of range.
-
classmethod
pure
(v)¶
-
reverse
()¶ S.reverse() – reverse IN PLACE
-
property
s
¶ - Parameters
q (Quaternion, UnitQuaternion) – input quaternion
- Returns
real part of quaternion
- Return type
float or numpy.ndarray
If the quaternion is of length one, a scalar float is returned.
If the quaternion is of length >1, a numpy array shape=(N,) is returned.
-
property
unit
¶ Return an equivalent unit quaternion Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: quaternion @return: equivalent unit quaternion
-
property
v
¶ - Parameters
q (Quaternion, UnitQuaternion) – input quaternion
- Returns
vector part of quaternion
- Return type
numpy ndarray
If the quaternion is of length one, a numpy array shape=(3,) is returned.
If the quaternion is of length >1, a numpy array shape=(N,3) is returned.
-
property
vec
¶ - Parameters
q (Quaternion, UnitQuaternion) – input quaternion
- Returns
quaternion expressed as a vector
- Return type
numpy ndarray
If the quaternion is of length one, a numpy array shape=(4,) is returned.
If the quaternion is of length >1, a numpy array shape=(N,4) is returned.
-
__mul__
(right)[source]¶ Multiply unit quaternion
- Parameters
left (UnitQuaternion, Quaternion, 3-vector, 3xN array, float) – left multiplicand
right – right multiplicand
- Returns
product
- Return type
- Raises
ValueError
Multiplicands
Product
left
right
type
result
UnitQuaternion
Quaternion
Quaternion
Hamilton product
UnitQuaternion
UnitQuaternion
UnitQuaternion
Hamilton product
UnitQuaternion
scalar
Quaternion
scalar product
UnitQuaternion
3-vector
3-vector
vector rotation
UnitQuaternion
3xN array
3xN array
vector rotations
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
left
right
len
operation
1
1
1
prod = left * right
1
N
N
prod[i] = left * right[i]
N
1
N
prod[i] = left[i] * right
N
N
N
prod[i] = left[i] * right[i]
N
M
ValueError
A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
- Seealso
__mul__()
-
interp
(s=0, dest=None, shortest=False)[source]¶ Algorithm source: https://en.wikipedia.org/wiki/Slerp :param qr: UnitQuaternion :param shortest: Take the shortest path along the great circle :param s: interpolation in range [0,1] :type s: float :return: interpolated UnitQuaternion
-
property
rpy
¶
-
property
eul
¶
-
property
angvec
¶
-
property
SO3
¶
-
property
SE3
¶
-
Geometry¶
Geometry in 3D¶
-
class
spatialmath.geom3d.
Plane
(c)[source]¶ Bases:
object
Create a plane object from linear coefficients
- Parameters
c (4-element array_like) – Plane coefficients
- Returns
a Plane object
- Return type
Planes are represented by the 4-vector \([a, b, c, d]\) which describes the plane \(\pi: ax + by + cz + d=0\).
-
static
PN
(p, n)[source]¶ Create a plane object from point and normal
- Parameters
p (3-element array_like) – Point in the plane
n (3-element array_like) – Normal to the plane
- Returns
a Plane object
- Return type
-
static
P3
(p)[source]¶ Create a plane object from three points
- Parameters
p (numpy.ndarray, shape=(3,3)) – Three points in the plane
- Returns
a Plane object
- Return type
-
property
n
¶ Normal to the plane
- Returns
Normal to the plane
- Return type
3-element array_like
For a plane \(\pi: ax + by + cz + d=0\) this is the vector \([a,b,c]\).
-
property
d
¶ Plane offset
- Returns
Offset of the plane
- Return type
float
For a plane \(\pi: ax + by + cz + d=0\) this is the scalar \(d\).
-
contains
(p, tol=2.220446049250313e-15)[source]¶ - Parameters
p (3-element array_like) – A 3D point
tol (float, optional) – Tolerance, defaults to 10*_eps
- Returns
if the point is in the plane
- Return type
bool
-
__eq__
()¶ Return self==value.
-
__ne__
()¶ Return self!=value.
-
class
spatialmath.geom3d.
Plucker
(v=None, w=None)[source]¶ Bases:
collections.UserList
Plucker coordinate class
Concrete class to represent a 3D line using Plucker coordinates.
Methods:
Plucker Contructor from points Plucker.planes Constructor from planes Plucker.pointdir Constructor from point and direction
Information and test methods:: closest closest point on line commonperp common perpendicular for two lines contains test if point is on line distance minimum distance between two lines intersects intersection point for two lines intersect_plane intersection points with a plane intersect_volume intersection points with a volume pp principal point ppd principal point distance from origin point generate point on line
Conversion methods:: char convert to human readable string double convert to 6-vector skew convert to 4x4 skew symmetric matrix
Display and print methods:: display display in human readable form plot plot line
Operators: * multiply Plucker matrix by a general matrix | test if lines are parallel ^ test if lines intersect == test if two lines are equivalent ~= test if lines are not equivalent
Notes:
This is reference (handle) class object
Plucker objects can be used in vectors and arrays
References:
Ken Shoemake, “Ray Tracing News”, Volume 11, Number 1 http://www.realtimerendering.com/resources/RTNews/html/rtnv11n1.html#art3
Matt Mason lecture notes http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lectures/lecture9.pdf
Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p596-7.
Implementation notes:
The internal representation is a 6-vector [v, w] where v (moment), w (direction).
There is a huge variety of notation used across the literature, as well as the ordering of the direction and moment components in the 6-vector.
Copyright (C) 1993-2019 Peter I. Corke
-
__init__
(v=None, w=None)[source]¶ Create a Plucker 3D line object
- Parameters
v (6-element array_like, Plucker instance, 3-element array_like) – Plucker vector, Plucker object, Plucker moment
w (3-element array_like, optional) – Plucker direction, optional
- Raises
ValueError – bad arguments
- Returns
Plucker line
- Return type
L = Plucker(X)
creates a Plucker object from the Plucker coordinate vectorX
= [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 objectL
.L = Plucker(V, W)
creates a Plucker object from momentV
(3-vector) and line directionW
(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]
orL[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
-
static
PQ
(P=None, Q=None)[source]¶ Create Plucker line object from two 3D points
- Parameters
P (3-element array_like) – First 3D point
Q (3-element array_like) – Second 3D point
- Returns
Plucker line
- Return type
L = Plucker(P, Q)
create a Plucker object that represents the line joining the 3D pointsP
(3-vector) andQ
(3-vector). The direction is fromQ
toP
.- Seealso
Plucker, Plucker.Planes, Plucker.PointDir
-
static
Planes
(pi1, pi2)[source]¶ Create Plucker line from two planes
- Parameters
- Returns
Plucker line
- Return type
L = Plucker.planes(PI1, PI2)
is a Plucker object that represents the line formed by the intersection of two planesPI1
andPI2
.Planes are represented by the 4-vector \([a, b, c, d]\) which describes the plane \(\pi: ax + by + cz + d=0\).
- Seealso
Plucker, Plucker.PQ, Plucker.PointDir
-
static
PointDir
(point, dir)[source]¶ Create Plucker line from point and direction
- Parameters
point (3-element array_like) – A 3D point
dir (3-element array_like) – Direction vector
- Returns
Plucker line
- Return type
L = Plucker.pointdir(P, W)
is a Plucker object that represents the line containing the pointP
and parallel to the direction vectorW
.- Seealso
Plucker, Plucker.Planes, Plucker.PQ
-
property
A
¶
-
property
v
¶ Moment vector
- Returns
the moment vector
- Return type
numpy.ndarray, shape=(3,)
-
property
w
¶ Direction vector
- Returns
the direction vector
- Return type
numpy.ndarray, shape=(3,)
- Seealso
Plucker.uw
-
property
uw
¶ Line direction as a unit vector
- Returns
Line direction
- Return type
numpy.ndarray, shape=(3,)
line.uw
is a unit-vector parallel to the line.
-
property
vec
¶ Line as a Plucker coordinate vector
- Returns
Coordinate vector
- Return type
numpy.ndarray, shape=(6,)
line.vec
is the Plucker coordinate vectorX
= [V,W] where V (3-vector) is the moment and W (3-vector) is the line direction.
-
property
skew
¶ Line as a Plucker skew-matrix
- Returns
Skew-symmetric matrix form of Plucker coordinates
- Return type
numpy.ndarray, shape=(4,4)
M = line.skew()
is the Plucker matrix, a 4x4 skew-symmetric matrix representation of the line.Notes:
For two homogeneous points P and Q on the line, \(PQ^T-QP^T\) is also skew symmetric.
The projection of Plucker line by a perspective camera is a homogeneous line (3x1) given by \(\vee C M C^T\) where \(C \in \mathbf{R}^{3 \times 4}\) is the camera matrix.
-
property
pp
¶ Principal point of the line
line.pp
is the point on the line that is closest to the origin.Notes:
Same as Plucker.point(0)
- Seealso
Plucker.ppd, Plucker.point
-
property
ppd
¶ Distance from principal point to the origin
- Returns
Distance from principal point to the origin
- Return type
float
line.ppd
is the distance from the principal point to the origin. This is the smallest distance of any point on the line to the origin.- Seealso
Plucker.pp
-
point
(lam)[source]¶ Generate point on line
- Parameters
lam (float) – Scalar distance from principal point
- Returns
Distance from principal point to the origin
- Return type
float
line.point(LAMBDA)
is a point on the line, whereLAMBDA
is the parametric distance along the line from the principal point of the line such that \(P = P_p + \lambda \hat{d}\) and \(\hat{d}\) is the line direction given byline.uw
.- Seealso
Plucker.pp, Plucker.closest, Plucker.uw
-
contains
(x, tol=1.1102230246251565e-14)[source]¶ Test if points are on the line
- Parameters
x (3-element array_like, or numpy.ndarray, shape=(3,N)) – 3D point
tol (float, optional) – Tolerance, defaults to 50*_eps
- Raises
ValueError – Bad argument
- Returns
Whether point is on the line
- Return type
bool or numpy.ndarray(N) of bool
line.contains(X)
is true if the pointX
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.
-
__eq__
(l2)[source]¶ Test if two lines are equivalent
- Parameters
- Returns
Plucker
- Returns
line equivalence
- Return type
bool
L1 == L2
is true if the Plucker objects describe the same line in space. Note that because of the over parameterization, lines can be equivalent even if their coordinate vectors are different.
-
__ne__
(l2)[source]¶ Test if two lines are not equivalent
- Parameters
- Returns
line inequivalence
- Return type
bool
L1 != L2
is true if the Plucker objects describe different lines in space. Note that because of the over parameterization, lines can be equivalent even if their coordinate vectors are different.
-
isparallel
(l2, tol=2.220446049250313e-15)[source]¶ Test if lines are parallel
- Parameters
- Returns
lines are parallel
- Return type
bool
l1.isparallel(l2)
is true if the two lines are parallel.l1 | l2
as above but in binary operator form- Seealso
Plucker.or, Plucker.intersects
-
__or__
(l2)[source]¶ Test if lines are parallel as a binary operator
- Parameters
- Returns
lines are parallel
- Return type
bool
l1 | l2
is an operator which is true if the two lines are parallel.- Seealso
Plucker.isparallel, Plucker.__xor__
-
__xor__
(l2)[source]¶ Test if lines intersect as a binary operator
- Parameters
- Returns
lines intersect
- Return type
bool
l1 ^ l2
is an operator which is true if the two lines intersect at a point.Notes:
Is false if the lines are equivalent since they would intersect at an infinite number of points.
- Seealso
Plucker.intersects, Plucker.parallel
-
intersects
(l2)[source]¶ Intersection point of two lines
- Parameters
- Returns
3D intersection point
- Return type
numpy.ndarray, shape=(3,) or None
l1.intersects(l2)
is the point of intersection of the two lines, orNone
if the lines do not intersect or are equivalent.- Seealso
Plucker.commonperp, Plucker.eq, Plucker.__xor__
-
distance
(l2)[source]¶ Minimum distance between lines
- Parameters
- Returns
Closest distance
- Return type
float
``l1.distance(l2) is the minimum distance between two lines.
Notes:
Works for parallel, skew and intersecting lines.
-
closest
(x)[source]¶ Point on line closest to given point
- Parameters
line – A line
l2 (3-element array_like) – An arbitrary 3D point
- Returns
Point on the line and distance to line
- Return type
collections.namedtuple
line.closest(x).p
is the coordinate of a point on the line that is closest tox
.line.closest(x).d
is the distance between the point on the line andx
.
The return value is a named tuple with elements:
.p
for the point on the line as a numpy.ndarray, shape=(3,).d
for the distance to the point fromx
.lam
the lambda value for the point on the line.
- Seealso
Plucker.point
-
commonperp
(l2)[source]¶ Common perpendicular to two lines
- Parameters
- Returns
Perpendicular line
- Return type
Plucker or None
l1.commonperp(l2)
is the common perpendicular line between the two lines. ReturnsNone
if the lines are parallel.- Seealso
Plucker.intersect
-
__mul__
(right)[source]¶ Reciprocal product
- Parameters
- Returns
reciprocal product
- Return type
float
left * right
is the scalar reciprocal product \(\hat{w}_L \dot m_R + \hat{w}_R \dot m_R\).Notes:
Multiplication or composition of Plucker lines is not defined.
Pre-multiplication by an SE3 object is supported, see
__rmul__
.
- Seealso
Plucker.__rmul__
-
__rmul__
(left)[source]¶ Line transformation
- Parameters
- Returns
transformed line
- Return type
T * line
is the line transformed by the rigid body transformationT
.- Seealso
Plucker.__mul__
-
intersect_plane
(plane)[source]¶ Line intersection with a plane
- Parameters
- Returns
Intersection point
- Return type
collections.namedtuple
line.intersect_plane(plane).p
is the point where the line intersects the plane, or None if no intersection.line.intersect_plane(plane).lam
is the lambda value for the point on the line that intersects the plane.
The plane can be specified as:
a 4-vector \([a, b, c, d]\) which describes the plane \(\pi: ax + by + cz + d=0\).
a
Plane
object
The return value is a named tuple with elements:
.p
for the point on the line as a numpy.ndarray, shape=(3,).lam
the lambda value for the point on the line.
See also Plucker.point.
-
intersect_volume
(bounds)[source]¶ Line intersection with a volume
- Parameters
line (Plucker) – A line
bounds – Bounds of an axis-aligned rectangular cuboid
- Returns
Intersection point
- Return type
collections.namedtuple
line.intersect_volume(bounds).p
is a matrix (3xN) with columns that indicate where the line intersects the faces of the volume specified bybounds
= [xmin xmax ymin ymax zmin zmax]. The number of columns N is either:0, when the line is outside the plot volume or,
2 when the line pierces the bounding volume.
line.intersect_volume(bounds).lam
is an array of shape=(N,) where N is as above.The return value is a named tuple with elements:
.p
for the points on the line as a numpy.ndarray, shape=(3,N).lam
for the lambda values for the intersection points as a numpy.ndarray, shape=(N,).
See also Plucker.plot, Plucker.point.
-
plot
(bounds=None, **kwargs)[source]¶ Plot a line
- Parameters
- Returns
Plotted line
- Return type
Line3D or None
line.plot(bounds)
adds a line segment to the current axes, and the handle of the line is returned. The line segment is defined by the intersection of the line and the given rectangular cuboid. If the line does not intersect the plotting volume None is returned.line.plot()
as above but the bounds are taken from the axis limits of the current axes.
The line color or style is specified by:
a MATLAB-style linestyle like ‘k–’
additional arguments passed to Line2D
- Seealso
Plucker.intersect_volume
-
clear
() → None -- remove all items from S¶
-
copy
()¶
-
count
(value) → integer -- return number of occurrences of value¶
-
extend
(other)¶ S.extend(iterable) – extend sequence by appending elements from the iterable
-
index
(value[, start[, stop]]) → integer -- return first index of value.¶ Raises ValueError if the value is not present.
Supporting start and stop arguments is optional, but recommended.
-
insert
(i, item)¶ S.insert(index, value) – insert value before index
-
pop
([index]) → item -- remove and return item at index (default last).¶ Raise IndexError if list is empty or index is out of range.
-
remove
(item)¶ S.remove(value) – remove first occurrence of value. Raise ValueError if the value is not present.
-
reverse
()¶ S.reverse() – reverse IN PLACE
-
sort
(*args, **kwds)¶
Functions (base)¶
Transforms in 2D¶
This modules contains functions to create and transform rotation matrices and homogeneous tranformation matrices.
Vector arguments are what numpy refers to as array_like
and can be a list,
tuple, numpy array, numpy row vector or numpy column vector.
-
spatialmath.base.transforms2d.
rot2
(theta, unit='rad')[source]¶ Create SO(2) rotation
- Parameters
theta (float) – rotation angle
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
2x2 rotation matrix
- Return type
numpy.ndarray, shape=(2,2)
ROT2(THETA)
is an SO(2) rotation matrix (2x2) representing a rotation of THETA radians.ROT2(THETA, 'deg')
as above but THETA is in degrees.
-
spatialmath.base.transforms2d.
trot2
(theta, unit='rad', t=None)[source]¶ Create SE(2) pure rotation
- Parameters
theta (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
t (array_like :return: 3x3 homogeneous transformation matrix) – translation 2-vector, defaults to [0,0]
- Return type
numpy.ndarray, shape=(3,3)
TROT2(THETA)
is a homogeneous transformation (3x3) representing a rotation of THETA radians.TROT2(THETA, 'deg')
as above but THETA is in degrees.
Notes: - Translational component is zero.
-
spatialmath.base.transforms2d.
transl2
(x, y=None)[source]¶ Create SE(2) pure translation, or extract translation from SE(2) matrix
- Parameters
x (float) – translation along X-axis
y (float) – translation along Y-axis
- Returns
homogeneous transform matrix or the translation elements of a homogeneous transform
- Return type
numpy.ndarray, shape=(3,3)
Create a translational SE(2) matrix:
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.
Extract the translational part of an SE(2) matrix:
P = TRANSL2(T) is the translational part of a homogeneous transform as a 2-element numpy array.
-
spatialmath.base.transforms2d.
ishom2
(T, check=False)[source]¶ Test if matrix belongs to SE(2)
- Parameters
T (numpy.ndarray) – matrix to test
check (bool) – check validity of rotation submatrix
- Returns
whether matrix is an SE(2) homogeneous transformation matrix
- Return type
bool
ISHOM2(T)
is True if the argumentT
is of dimension 3x3ISHOM2(T, check=True)
as above, but also checks orthogonality of the rotation sub-matrix and validitity of the bottom row.
- Seealso
isR, isrot2, ishom, isvec
-
spatialmath.base.transforms2d.
isrot2
(R, check=False)[source]¶ Test if matrix belongs to SO(2)
- Parameters
R (numpy.ndarray) – matrix to test
check (bool) – check validity of rotation submatrix
- Returns
whether matrix is an SO(2) rotation matrix
- Return type
bool
ISROT(R)
is True if the argumentR
is of dimension 2x2ISROT(R, check=True)
as above, but also checks orthogonality of the rotation matrix.
- Seealso
isR, ishom2, isrot
-
spatialmath.base.transforms2d.
trlog2
(T, check=True)[source]¶ Logarithm of SO(2) or SE(2) matrix
- Parameters
T (numpy.ndarray, shape=(2,2) or (3,3)) – SO(2) or SE(2) matrix
- Returns
logarithm
- Return type
numpy.ndarray, shape=(2,2) or (3,3)
- Raises
ValueError
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 matrixR
which will be 2x2 skew-symmetric matrix. The equivalent vector fromvex()
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 matrixT
which will be 3x3 augumented skew-symmetric matrix. The equivalent vector fromvexa()
is the twist vector (6x1) comprising [v w].
-
spatialmath.base.transforms2d.
trexp2
(S, theta=None)[source]¶ Exponential of so(2) or se(2) matrix
- Parameters
S – so(2), se(2) matrix or equivalent velctor
theta (float) – motion
- Returns
2x2 or 3x3 matrix exponential in SO(2) or SE(2)
- Return type
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) elementS
which is a 2x2 skew-symmetric matrix.trexp2(S, THETA)
as above but for an so(3) motion of S*THETA, whereS
is unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude given byTHETA
.trexp2(W)
is the matrix exponential of the so(2) elementW
expressed as a 1-vector (array_like).trexp2(W, THETA)
as above but for an so(3) motion of W*THETA whereW
is a unit-norm vector representing a rotation axis and a rotation magnitude given byTHETA
.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) elementSIGMA
which is a 3x3 augmented skew-symmetric matrix.trexp2(SIGMA, THETA)
as above but for an se(3) motion of SIGMA*THETA, whereSIGMA
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) elementTW
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, whereTW
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix.
- seealso
trlog, trexp2
-
spatialmath.base.transforms2d.
trinterp2
(T0, T1=None, s=None)[source]¶ Interpolate SE(2) matrices
- Parameters
T0 (np.ndarray, shape=(3,3)) – first SE(2) matrix
T1 (np.ndarray, shape=(3,3)) – second SE(2) matrix
s (float) – interpolation coefficient, range 0 to 1
- Returns
SE(2) matrix
- Return type
np.ndarray, shape=(3,3)
trinterp2(T0, T1, S)
is a homogeneous transform (3x3) interpolated between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous transforms (3x3).trinterp2(T1, S)
as above but interpolated between the identity matrix when S=0 to T1 when S=1.
Notes:
Rotation angle is linearly interpolated.
- Seealso
%## 2d homogeneous trajectory
-
spatialmath.base.transforms2d.
trprint2
(T, label=None, file=<_io.TextIOWrapper name='<stdout>' mode='w' encoding='UTF-8'>, fmt='{:8.2g}', unit='deg')[source]¶ Compact display of SO(2) or SE(2) matrices
- Parameters
T (numpy.ndarray, shape=(2,2) or (3,3)) – matrix to format
label (str) – text label to put at start of line
file (str) – file to write formatted string to
fmt (str) – conversion format for each number
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
optional formatted string
- Return type
str
The matrix is formatted and written to
file
or iffile=None
then the string is returned.trprint2(R)
displays the SO(2) rotation matrix in a compact single-line format:[LABEL:] THETA UNIT
trprint2(T)
displays the SE(2) homogoneous transform in a compact single-line format:[LABEL:] [t=X, Y;] THETA UNIT
Example:
>>> T = transl2(1,2)@trot2(0.3) >>> trprint2(a, file=None, label='T') 'T: t = 1, 2; 17 deg'
- Seealso
trprint
-
spatialmath.base.transforms2d.
trplot2
(T, axes=None, dims=None, color='blue', frame=None, textcolor=None, labels=['X', 'Y'], length=1, arrow=True, rviz=False, wtl=0.2, width=1, d1=0.05, d2=1.15, **kwargs)[source]¶ Plot a 2D coordinate frame
- Parameters
T – an SO(3) or SE(3) pose to be displayed as coordinate frame
axes (Axes3D reference) – the axes to plot into, defaults to current axes
dims (array_like) – dimension of plot volume as [xmin, xmax, ymin, ymax]
color (str) – color of the lines defining the frame
textcolor (str) – color of text labels for the frame, default color of lines above
frame (str) – label the frame, name is shown below the frame and as subscripts on the frame axis labels
labels (3-tuple of strings) – labels for the axes, defaults to X, Y and Z
length (float) – length of coordinate frame axes, default 1
arrow (bool) – show arrow heads, default True
wtl (float) – width-to-length ratio for arrows, default 0.2
rviz (bool) – show Rviz style arrows, default False
projection (str) – 3D projection: ortho [default] or persp
width (float) – width of lines, default 1
d1 – distance of frame axis label text from origin, default 1.15
- Type
numpy.ndarray, shape=(2,2) or (3,3)
Adds a 2D coordinate frame represented by the SO(2) or SE(2) matrix to the current axes.
If no current figure, one is created
If current figure, but no axes, a 3d Axes is created
Examples:
trplot2(T, frame=’A’) trplot2(T, frame=’A’, color=’green’) trplot2(T1, ‘labels’, ‘AB’);
-
spatialmath.base.transforms2d.
tranimate2
(T, **kwargs)[source]¶ Animate a 2D coordinate frame
- Parameters
T – an SO(2) or SE(2) pose to be displayed as coordinate frame
nframes (int) – number of steps in the animation [defaault 100]
repeat (bool) – animate in endless loop [default False]
interval (int) – number of milliseconds between frames [default 50]
movie (str) – name of file to write MP4 movie into
- Type
numpy.ndarray, shape=(2,2) or (3,3)
Animates a 2D coordinate frame moving from the world frame to a frame represented by the SO(2) or SE(2) matrix to the current axes.
If no current figure, one is created
If current figure, but no axes, a 3d Axes is created
Examples:
tranimate2(transl(1,2)@trot2(1), frame=’A’, arrow=False, dims=[0, 5]) tranimate2(transl(1,2)@trot2(1), frame=’A’, arrow=False, dims=[0, 5], movie=’spin.mp4’)
Transforms in 3D¶
This modules contains functions to create and transform 3D rotation matrices and homogeneous tranformation matrices.
Vector arguments are what numpy refers to as array_like
and can be a list,
tuple, numpy array, numpy row vector or numpy column vector.
TODO:
trinterp
trjac, trjac2
tranimate, tranimate2
-
spatialmath.base.transforms3d.
rotx
(theta, unit='rad')[source]¶ Create SO(3) rotation about X-axis
- Parameters
theta (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3x3 rotation matrix
- Return type
numpy.ndarray, shape=(3,3)
rotx(THETA)
is an SO(3) rotation matrix (3x3) representing a rotation of THETA radians about the x-axisrotx(THETA, "deg")
as above but THETA is in degrees
- Seealso
-
spatialmath.base.transforms3d.
roty
(theta, unit='rad')[source]¶ Create SO(3) rotation about Y-axis
- Parameters
theta (float) – rotation angle about Y-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3x3 rotation matrix
- Return type
numpy.ndarray, shape=(3,3)
roty(THETA)
is an SO(3) rotation matrix (3x3) representing a rotation of THETA radians about the y-axisroty(THETA, "deg")
as above but THETA is in degrees
- Seealso
-
spatialmath.base.transforms3d.
rotz
(theta, unit='rad')[source]¶ Create SO(3) rotation about Z-axis
- Parameters
theta (float) – rotation angle about Z-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3x3 rotation matrix
- Return type
numpy.ndarray, shape=(3,3)
rotz(THETA)
is an SO(3) rotation matrix (3x3) representing a rotation of THETA radians about the z-axisrotz(THETA, "deg")
as above but THETA is in degrees
- Seealso
yrotz()
-
spatialmath.base.transforms3d.
trotx
(theta, unit='rad', t=None)[source]¶ Create SE(3) pure rotation about X-axis
- Parameters
theta (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
t (array_like :return: 4x4 homogeneous transformation matrix) – translation 3-vector, defaults to [0,0,0]
- Return type
numpy.ndarray, shape=(4,4)
trotx(THETA)
is a homogeneous transformation (4x4) representing a rotation of THETA radians about the x-axis.trotx(THETA, 'deg')
as above but THETA is in degreestrotx(THETA, 'rad', t=[x,y,z])
as above with translation of [x,y,z]
- Seealso
-
spatialmath.base.transforms3d.
troty
(theta, unit='rad', t=None)[source]¶ Create SE(3) pure rotation about Y-axis
- Parameters
theta (float) – rotation angle about Y-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
t (array_like) – translation 3-vector, defaults to [0,0,0]
- Returns
4x4 homogeneous transformation matrix as a numpy array
- Return type
numpy.ndarray, shape=(4,4)
troty(THETA)
is a homogeneous transformation (4x4) representing a rotation of THETA radians about the y-axis.troty(THETA, 'deg')
as above but THETA is in degreestroty(THETA, 'rad', t=[x,y,z])
as above with translation of [x,y,z]
- Seealso
-
spatialmath.base.transforms3d.
trotz
(theta, unit='rad', t=None)[source]¶ Create SE(3) pure rotation about Z-axis
- Parameters
theta (float) – rotation angle about Z-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
t (array_like) – translation 3-vector, defaults to [0,0,0]
- Returns
4x4 homogeneous transformation matrix
- Return type
numpy.ndarray, shape=(4,4)
trotz(THETA)
is a homogeneous transformation (4x4) representing a rotation of THETA radians about the z-axis.trotz(THETA, 'deg')
as above but THETA is in degreestrotz(THETA, 'rad', t=[x,y,z])
as above with translation of [x,y,z]
- Seealso
-
spatialmath.base.transforms3d.
transl
(x, y=None, z=None)[source]¶ Create SE(3) pure translation, or extract translation from SE(3) matrix
- Parameters
x (float) – translation along X-axis
y (float) – translation along Y-axis
z (float) – translation along Z-axis
- Returns
4x4 homogeneous transformation matrix
- Return type
numpy.ndarray, shape=(4,4)
Create a translational SE(3) matrix:
T = transl( X, Y, Z )
is an SE(3) homogeneous transform (4x4) representing a pure translation of X, Y and Z.T = transl( V )
as above but the translation is given by a 3-element list, dict, or a numpy array, row or column vector.
Extract the translational part of an SE(3) matrix:
P = TRANSL(T)
is the translational part of a homogeneous transform T as a 3-element numpy array.
- Seealso
-
spatialmath.base.transforms3d.
ishom
(T, check=False, tol=10)[source]¶ Test if matrix belongs to SE(3)
- Parameters
T (numpy.ndarray) – matrix to test
check (bool) – check validity of rotation submatrix
- Returns
whether matrix is an SE(3) homogeneous transformation matrix
- Return type
bool
ISHOM(T)
is True if the argumentT
is of dimension 4x4ISHOM(T, check=True)
as above, but also checks orthogonality of the rotation sub-matrix and validitity of the bottom row.
-
spatialmath.base.transforms3d.
isrot
(R, check=False, tol=10)[source]¶ Test if matrix belongs to SO(3)
- Parameters
R (numpy.ndarray) – matrix to test
check (bool) – check validity of rotation submatrix
- Returns
whether matrix is an SO(3) rotation matrix
- Return type
bool
ISROT(R)
is True if the argumentR
is of dimension 3x3ISROT(R, check=True)
as above, but also checks orthogonality of the rotation matrix.
-
spatialmath.base.transforms3d.
rpy2r
(roll, pitch=None, yaw=None, *, unit='rad', order='zyx')[source]¶ Create an SO(3) rotation matrix from roll-pitch-yaw angles
- Parameters
roll (float) – roll angle
pitch (float) – pitch angle
yaw (float) – yaw angle
unit (str) – angular units: ‘rad’ [default], or ‘deg’
unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns
3x3 rotation matrix
- Return type
numpdy.ndarray, shape=(3,3)
rpy2r(ROLL, PITCH, YAW)
is an SO(3) orthonormal rotation matrix (3x3) equivalent to the specified roll, pitch, yaw angles angles. These correspond to successive rotations about the axes specified byorder
:‘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. Covention 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.
rpy2r(RPY)
as above but the roll, pitch, yaw angles are taken fromRPY
which is a 3-vector (array_like) with values (ROLL, PITCH, YAW).
-
spatialmath.base.transforms3d.
rpy2tr
(roll, pitch=None, yaw=None, unit='rad', order='zyx')[source]¶ Create an SE(3) rotation matrix from roll-pitch-yaw angles
- Parameters
roll (float) – roll angle
pitch (float) – pitch angle
yaw (float) – yaw angle
unit (str) – angular units: ‘rad’ [default], or ‘deg’
unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns
3x3 rotation matrix
- Return type
numpdy.ndarray, shape=(3,3)
rpy2tr(ROLL, PITCH, YAW)
is an SO(3) orthonormal rotation matrix (3x3) equivalent to the specified roll, pitch, yaw angles angles. These correspond to successive rotations about the axes specified byorder
:‘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.
rpy2tr(RPY)
as above but the roll, pitch, yaw angles are taken fromRPY
which is a 3-vector (array_like) with values (ROLL, PITCH, YAW).
Notes:
The translational part is zero.
-
spatialmath.base.transforms3d.
eul2r
(phi, theta=None, psi=None, unit='rad')[source]¶ Create an SO(3) rotation matrix from Euler angles
- Parameters
phi (float) – Z-axis rotation
theta (float) – Y-axis rotation
psi (float) – Z-axis rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
3x3 rotation matrix
- Return type
numpdy.ndarray, shape=(3,3)
R = eul2r(PHI, THETA, PSI)
is an SO(3) orthonornal rotation matrix equivalent to the specified Euler angles. These correspond to rotations about the Z, Y, Z axes respectively.R = eul2r(EUL)
as above but the Euler angles are taken fromEUL
which is a 3-vector (array_like) with values (PHI THETA PSI).
-
spatialmath.base.transforms3d.
eul2tr
(phi, theta=None, psi=None, unit='rad')[source]¶ Create an SE(3) pure rotation matrix from Euler angles
- Parameters
phi (float) – Z-axis rotation
theta (float) – Y-axis rotation
psi (float) – Z-axis rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns
4x4 homogeneous transformation matrix
- Return type
numpdy.ndarray, shape=(4,4)
R = eul2tr(PHI, THETA, PSI)
is an SE(3) homogeneous transformation matrix equivalent to the specified Euler angles. These correspond to rotations about the Z, Y, Z axes respectively.R = eul2tr(EUL)
as above but the Euler angles are taken fromEUL
which is a 3-vector (array_like) with values (PHI THETA PSI).
Notes:
The translational part is zero.
-
spatialmath.base.transforms3d.
angvec2r
(theta, v, unit='rad')[source]¶ Create an SO(3) rotation matrix from rotation angle and axis
- Parameters
theta (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (array_like) – rotation axis, 3-vector
- Returns
3x3 rotation matrix
- Return type
numpdy.ndarray, shape=(3,3)
angvec2r(THETA, V)
is an SO(3) orthonormal rotation matrix equivalent to a rotation ofTHETA
about the vectorV
.Notes:
If
THETA == 0
then return identity matrix.If
THETA ~= 0
thenV
must have a finite length.
- Seealso
-
spatialmath.base.transforms3d.
angvec2tr
(theta, v, unit='rad')[source]¶ Create an SE(3) pure rotation from rotation angle and axis
- Parameters
theta (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (: array_like) – rotation axis, 3-vector
- Returns
4x4 homogeneous transformation matrix
- Return type
numpdy.ndarray, shape=(4,4)
angvec2tr(THETA, V)
is an SE(3) homogeneous transformation matrix equivalent to a rotation ofTHETA
about the vectorV
.Notes:
If
THETA == 0
then return identity matrix.If
THETA ~= 0
thenV
must have a finite length.The translational part is zero.
- Seealso
-
spatialmath.base.transforms3d.
oa2r
(o, a=None)[source]¶ Create SO(3) rotation matrix from two vectors
- Parameters
o (array_like) – 3-vector parallel to Y- axis
a – 3-vector parallel to the Z-axis
- Returns
3x3 rotation matrix
- Return type
numpy.ndarray, shape=(3,3)
T = oa2tr(O, A)
is an SO(3) orthonormal rotation matrix for a frame defined in terms of vectors parallel to its Y- and Z-axes with respect to a reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N O A] and N = O x A.Steps:
N’ = O x A
O’ = A x N
normalize N’, O’, A
stack horizontally into rotation matrix
Notes:
The A vector is the only guaranteed to have the same direction in the resulting rotation matrix
O and A do not have to be unit-length, they are normalized
O and A do not have to be orthogonal, so long as they are not parallel
The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.
- Seealso
-
spatialmath.base.transforms3d.
oa2tr
(o, a=None)[source]¶ Create SE(3) pure rotation from two vectors
- Parameters
o (array_like) – 3-vector parallel to Y- axis
a – 3-vector parallel to the Z-axis
- Returns
4x4 homogeneous transformation matrix
- Return type
numpy.ndarray, shape=(4,4)
T = oa2tr(O, A)
is an SE(3) homogeneous transformation matrix for a frame defined in terms of vectors parallel to its Y- and Z-axes with respect to a reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N O A] and N = O x A.Steps:
N’ = O x A
O’ = A x N
normalize N’, O’, A
stack horizontally into rotation matrix
Notes:
The A vector is the only guaranteed to have the same direction in the resulting rotation matrix
O and A do not have to be unit-length, they are normalized
O and A do not have to be orthogonal, so long as they are not parallel
The translational part is zero.
The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.
- Seealso
-
spatialmath.base.transforms3d.
tr2angvec
(T, unit='rad', check=False)[source]¶ Convert SO(3) or SE(3) to angle and rotation vector
- Parameters
R (numpy.ndarray, shape=(3,3) or (4,4)) – SO(3) or SE(3) matrix
unit (str) – ‘rad’ or ‘deg’
check (bool) – check that rotation matrix is valid
- Returns
\((\theta, {\bf v})\)
- Return type
float, numpy.ndarray, shape=(3,)
tr2angvec(R)
is a rotation angle and a vector about which the rotation acts that corresponds to the rotation part ofR
.By default the angle is in radians but can be changed setting unit=’deg’.
Notes:
If the input is SE(3) the translation component is ignored.
- Seealso
-
spatialmath.base.transforms3d.
tr2eul
(T, unit='rad', flip=False, check=False)[source]¶ Convert SO(3) or SE(3) to ZYX Euler angles
- Parameters
R (numpy.ndarray, shape=(3,3) or (4,4)) – SO(3) or SE(3) matrix
unit (str) – ‘rad’ or ‘deg’
flip (bool) – choose first Euler angle to be in quadrant 2 or 3
check (bool) – check that rotation matrix is valid
- Returns
ZYZ Euler angles
- Return type
numpy.ndarray, shape=(3,)
tr2eul(R)
are the Euler angles corresponding to the rotation part ofR
.The 3 angles \([\phi, \theta, \psi\) correspond to sequential rotations about the Z, Y and Z axes respectively.
By default the angles are in radians but can be changed setting unit=’deg’.
Notes:
There is a singularity for the case where \(\theta=0\) in which case \(\phi\) is arbitrarily set to zero and \(\phi\) is set to \(\phi+\psi\).
If the input is SE(3) the translation component is ignored.
- Seealso
-
spatialmath.base.transforms3d.
tr2rpy
(T, unit='rad', order='zyx', check=False)[source]¶ Convert SO(3) or SE(3) to roll-pitch-yaw angles
- Parameters
R (numpy.ndarray, shape=(3,3) or (4,4)) – SO(3) or SE(3) matrix
unit (str) – ‘rad’ or ‘deg’
order – ‘xyz’, ‘zyx’ or ‘yxz’ [default ‘zyx’]
check (bool) – check that rotation matrix is valid
- Returns
Roll-pitch-yaw angles
- Return type
numpy.ndarray, shape=(3,)
tr2rpy(R)
are the roll-pitch-yaw angles corresponding to the rotation part ofR
.The 3 angles RPY=[R,P,Y] correspond to sequential rotations about the Z, Y and X axes respectively. The axis order sequence can be changed by setting:
order=’xyz’ for sequential rotations about X, Y, Z axes
order=’yxz’ for sequential rotations about Y, X, Z axes
By default the angles are in radians but can be changed setting unit=’deg’.
Notes:
There is a singularity for the case where P=:math:pi/2 in which case R is arbitrarily set to zero and Y is the sum (R+Y).
If the input is SE(3) the translation component is ignored.
- Seealso
-
spatialmath.base.transforms3d.
trlog
(T, check=True)[source]¶ Logarithm of SO(3) or SE(3) matrix
- Parameters
T (numpy.ndarray, shape=(3,3) or (4,4)) – SO(3) or SE(3) matrix
- Returns
logarithm
- Return type
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 matrixR
which will be 3x3 skew-symmetric matrix. The equivalent vector fromvex()
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 matrixT
which will be 4x4 augumented skew-symmetric matrix. The equivalent vector fromvexa()
is the twist vector (6x1) comprising [v w].
-
spatialmath.base.transforms3d.
trexp
(S, theta=None)[source]¶ Exponential of so(3) or se(3) matrix
- Parameters
S – so(3), se(3) matrix or equivalent velctor
theta (float) – motion
- Returns
3x3 or 4x4 matrix exponential in SO(3) or SE(3)
- Return type
numpy.ndarray, shape=(3,3) or (4,4)
An efficient closed-form solution of the matrix exponential for arguments that are so(3) or se(3).
For so(3) the results is an SO(3) rotation matrix:
trexp(S)
is the matrix exponential of the so(3) elementS
which is a 3x3skew-symmetric matrix.
trexp(S, THETA)
as above but for an so(3) motion of S*THETA, whereS
is unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude given byTHETA
.trexp(W)
is the matrix exponential of the so(3) elementW
expressed as a 3-vector (array_like).trexp(W, THETA)
as above but for an so(3) motion of W*THETA whereW
is a unit-norm vector representing a rotation axis and a rotation magnitude given byTHETA
.W
is expressed as a 3-vector (array_like).
For se(3) the results is an SE(3) homogeneous transformation matrix:
trexp(SIGMA)
is the matrix exponential of the se(3) elementSIGMA
which is a 4x4 augmented skew-symmetric matrix.trexp(SIGMA, THETA)
as above but for an se(3) motion of SIGMA*THETA, whereSIGMA
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix.trexp(TW)
is the matrix exponential of the se(3) elementTW
represented as a 6-vector which can be considered a screw motion.trexp(TW, THETA)
as above but for an se(3) motion of TW*THETA, whereTW
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix.
- seealso
trexp2()
-
spatialmath.base.transforms3d.
trnorm
(T)[source]¶ Normalize an SO(3) or SE(3) matrix
- Parameters
T – SO(3) or SE(3) matrix
T1 (np.ndarray, shape=(3,3) or (4,4)) – second SE(3) matrix
- Returns
SO(3) or SE(3) matrix
- Return type
np.ndarray, shape=(3,3) or (4,4)
trnorm(R)
is guaranteed to be a proper orthogonal matrix rotation matrix (3x3) which is “close” to the input matrix R (3x3). If R = [N,O,A] the O and A vectors are made unit length and the normal vector is formed from N = O x A, and then we ensure that O and A are orthogonal by O = A x N.trnorm(T)
as above but the rotational submatrix of the homogeneous transformation T (4x4) is normalised while the translational part is unchanged.
Notes:
Only the direction of A (the z-axis) is unchanged.
Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.
-
spatialmath.base.transforms3d.
trinterp
(T0, T1=None, s=None)[source]¶ Interpolate SE(3) matrices
- Parameters
T0 (np.ndarray, shape=(4,4)) – first SE(3) matrix
T1 (np.ndarray, shape=(4,4)) – second SE(3) matrix
s (float) – interpolation coefficient, range 0 to 1
- Returns
SE(3) matrix
- Return type
np.ndarray, shape=(4,4)
trinterp(T0, T1, S)
is a homogeneous transform (4x4) interpolated between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous transforms (4x4).trinterp(T1, S)
as above but interpolated between the identity matrix when S=0 to T1 when S=1.
Notes:
Rotation is interpolated using quaternion spherical linear interpolation (slerp).
- Seealso
spatialmath.base.quaternions.slerp()
,trinterp2()
-
spatialmath.base.transforms3d.
delta2tr
(d)[source]¶ Convert differential motion to SE(3)
- Parameters
d (array_like) – differential motion as a 6-vector
- Returns
SE(3) matrix
- Return type
np.ndarray, shape=(4,4)
T = delta2tr(d)
is an SE(3) matrix representing differential motion \(d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z\).Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
- Seealso
-
spatialmath.base.transforms3d.
trinv
(T)[source]¶ Invert an SE(3) matrix
- Parameters
T (np.ndarray, shape=(4,4)) – an SE(3) matrix
- Returns
SE(3) matrix
- Return type
np.ndarray, shape=(4,4)
Computes an efficient inverse of an SE(3) matrix:
\(\begin{pmatrix} {\bf R} & t \\ 0\,0\,0 & 1 \end{pmatrix}^{-1} = \begin{pmatrix} {\bf R}^T & -{\bf R}^T t \\ 0\,0\, 0 & 1 \end{pmatrix}\)
-
spatialmath.base.transforms3d.
tr2delta
(T0, T1=None)[source]¶ Difference of SE(3) matrices as differential motion
- Parameters
T0 (np.ndarray, shape=(4,4)) – first SE(3) matrix
T1 (np.ndarray, shape=(4,4)) – second SE(3) matrix
- Returns
Sdifferential motion as a 6-vector
- Return type
np.ndarray, shape=(6,)
tr2delta(T0, T1)
is the differential motion (6x1) corresponding to infinitessimal motion (in the T0 frame) from pose T0 to T1 which are SE(3) matrices.tr2delta(T)
as above but the motion is from the world frame to the pose represented by T.
The vector \(d = [\delta_x, \delta_y, \delta_z, heta_x, heta_y, heta_z\) represents infinitessimal translation and rotation, and is an approximation to the instantaneous spatial velocity multiplied by time step.
Notes:
D is only an approximation to the motion T, and assumes that T0 ~ T1 or T ~ eye(4,4).
Can be considered as an approximation to the effect of spatial velocity over a a time interval, average spatial velocity multiplied by time.
Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
- Seealso
-
spatialmath.base.transforms3d.
tr2jac
(T, samebody=False)[source]¶ SE(3) adjoint
- Parameters
T (np.ndarray, shape=(4,4)) – an SE(3) matrix
- Returns
adjoint matrix
- Return type
np.ndarray, shape=(6,6)
Computes an adjoint matrix that maps spatial velocity between two frames defined by an SE(3) matrix. It acts like a Jacobian matrix.
tr2jac(T)
is a Jacobian matrix (6x6) that maps spatial velocity or differential motion from frame {A} to frame {B} where the pose of {B} relative to {A} is represented by the homogeneous transform T = \({}^A {f T}_B\).tr2jac(T, True)
as above but for the case when frame {A} to frame {B} are both attached to the same moving body.
-
spatialmath.base.transforms3d.
trprint
(T, orient='rpy/zyx', label=None, file=<_io.TextIOWrapper name='<stdout>' mode='w' encoding='UTF-8'>, fmt='{:8.2g}', unit='deg')[source]¶ Compact display of SO(3) or SE(3) matrices
- param T
matrix to format
- type T
numpy.ndarray, shape=(3,3) or (4,4)
- param label
text label to put at start of line
- type label
str
- param orient
3-angle convention to use
- type orient
str
- param file
file to write formatted string to. [default, stdout]
- type file
str
- param fmt
conversion format for each number
- type fmt
str
- param unit
angular units: ‘rad’ [default], or ‘deg’
- type unit
str
- return
optional formatted string
- rtype
str
The matrix is formatted and written to
file
or iffile=None
then the string is returned.trprint(R)
displays the SO(3) rotation matrix in a compactsingle-line format:
[LABEL:] ORIENTATION UNIT
trprint(T)
displays the SE(3) homogoneous transform in a compact single-line format:[LABEL:] [t=X, Y, Z;] ORIENTATION UNIT
Orientation is expressed in one of several formats:
‘rpy/zyx’ roll-pitch-yaw angles in ZYX axis order [default]
‘rpy/yxz’ roll-pitch-yaw angles in YXZ axis order
‘rpy/zyx’ roll-pitch-yaw angles in ZYX axis order
‘eul’ Euler angles in ZYZ axis order
‘angvec’ angle and axis
Example:
>>> T = transl(1,2,3) @ rpy2tr(10, 20, 30, 'deg') >>> trprint(T, file=None, label='T') 'T: t = 1, 2, 3; rpy/zyx = 10, 20, 30 deg' >>> trprint(T, file=None, label='T', orient='angvec') 'T: t = 1, 2, 3; angvec = ( 56 deg | 0.12, 0.62, 0.78)' >>> trprint(T, file=None, label='T', orient='angvec', fmt='{:8.4g}') 'T: t = 1, 2, 3; angvec = ( 56.04 deg | 0.124, 0.6156, 0.7782)'
Notes:
If the ‘rpy’ option is selected, then the particular angle sequence can be specified with the options ‘xyz’ or ‘yxz’ which are passed through to
tr2rpy
. ‘zyx’ is the default.Default formatting is for readable columns of data
- seealso
-
spatialmath.base.transforms3d.
trplot
(T, axes=None, dims=None, color='blue', frame=None, textcolor=None, labels=['X', 'Y', 'Z'], length=1, arrow=True, projection='ortho', rviz=False, wtl=0.2, width=1, d1=0.05, d2=1.15, **kwargs)[source]¶ Plot a 3D coordinate frame
- Parameters
T – an SO(3) or SE(3) pose to be displayed as coordinate frame
axes (Axes3D reference) – the axes to plot into, defaults to current axes
dims (array_like) – dimension of plot volume as [xmin, xmax, ymin, ymax,zmin, zmax]. If dims is [min, max] those limits are applied to the x-, y- and z-axes.
color (str) – color of the lines defining the frame
textcolor (str) – color of text labels for the frame, default color of lines above
frame (str) – label the frame, name is shown below the frame and as subscripts on the frame axis labels
labels (3-tuple of strings) – labels for the axes, defaults to X, Y and Z
length (float) – length of coordinate frame axes, default 1
arrow (bool) – show arrow heads, default True
wtl (float) – width-to-length ratio for arrows, default 0.2
rviz (bool) – show Rviz style arrows, default False
projection (str) – 3D projection: ortho [default] or persp
width (float) – width of lines, default 1
d1 – distance of frame axis label text from origin, default 1.15
- Type
numpy.ndarray, shape=(3,3) or (4,4)
Adds a 3D coordinate frame represented by the SO(3) or SE(3) matrix to the current axes.
If no current figure, one is created
If current figure, but no axes, a 3d Axes is created
Examples:
trplot(T, frame=’A’) trplot(T, frame=’A’, color=’green’) trplot(T1, ‘labels’, ‘NOA’);
-
spatialmath.base.transforms3d.
tranimate
(T, **kwargs)[source]¶ Animate a 3D coordinate frame
- Parameters
T – an SO(3) or SE(3) pose to be displayed as coordinate frame
nframes (int) – number of steps in the animation [defaault 100]
repeat (bool) – animate in endless loop [default False]
interval (int) – number of milliseconds between frames [default 50]
movie (str) – name of file to write MP4 movie into
- Type
numpy.ndarray, shape=(3,3) or (4,4)
Animates a 3D coordinate frame moving from the world frame to a frame represented by the SO(3) or SE(3) matrix to the current axes.
If no current figure, one is created
If current figure, but no axes, a 3d Axes is created
Examples:
tranimate(transl(1,2,3)@trotx(1), frame=’A’, arrow=False, dims=[0, 5]) tranimate(transl(1,2,3)@trotx(1), frame=’A’, arrow=False, dims=[0, 5], movie=’spin.mp4’)
Transforms in ND¶
This modules contains functions to create and transform rotation matrices and homogeneous tranformation matrices.
Vector arguments are what numpy refers to as array_like
and can be a list,
tuple, numpy array, numpy row vector or numpy column vector.
Versions:
Luis Fernando Lara Tobar and Peter Corke, 2008
Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017
Peter Corke, 2020
-
spatialmath.base.transformsNd.
r2t
(R, check=False)[source]¶ Convert SO(n) to SE(n)
- Parameters
R – rotation matrix
check – check if rotation matrix is valid (default False, no check)
- Returns
homogeneous transformation matrix
- Return type
numpy.ndarray, shape=(3,3) or (4,4)
T = r2t(R)
is an SE(2) or SE(3) homogeneous transform equivalent to an SO(2) or SO(3) orthonormal rotation matrixR
with a zero translational componentif
R
is 2x2 thenT
is 3x3: SO(2) -> SE(2)if
R
is 3x3 thenT
is 4x4: SO(3) -> SE(3)
- Seealso
t2r, rt2tr
-
spatialmath.base.transformsNd.
t2r
(T, check=False)[source]¶ Convert SE(n) to SO(n)
- Parameters
T – homogeneous transformation matrix
check – check if rotation matrix is valid (default False, no check)
- Returns
rotation matrix
- Return type
numpy.ndarray, shape=(2,2) or (3,3)
R = T2R(T)
is the orthonormal rotation matrix component of homogeneous transformation matrixT
if
T
is 3x3 thenR
is 2x2: SE(2) -> SO(2)if
T
is 4x4 thenR
is 3x3: SE(3) -> SO(3)
Any translational component of T is lost.
- Seealso
r2t, tr2rt
-
spatialmath.base.transformsNd.
tr2rt
(T, check=False)[source]¶ Convert SE(3) to SO(3) and translation
- Parameters
T – homogeneous transform matrix
check – check if rotation matrix is valid (default False, no check)
- Returns
Rotation matrix and translation vector
- Return type
tuple: numpy.ndarray, shape=(2,2) or (3,3); numpy.ndarray, shape=(2,) or (3,)
(R,t) = tr2rt(T) splits a homogeneous transformation matrix (NxN) into an orthonormal rotation matrix R (MxM) and a translation vector T (Mx1), where N=M+1.
if
T
is 3x3 - in SE(2) - thenR
is 2x2 andt
is 2x1.if
T
is 4x4 - in SE(3) - thenR
is 3x3 andt
is 3x1.
- Seealso
rt2tr, tr2r
-
spatialmath.base.transformsNd.
rt2tr
(R, t, check=False)[source]¶ Convert SO(3) and translation to SE(3)
- Parameters
R – rotation matrix
t – translation vector
check – check if rotation matrix is valid (default False, no check)
- Returns
homogeneous transform
- Return type
numpy.ndarray, shape=(3,3) or (4,4)
T = rt2tr(R, t)
is a homogeneous transformation matrix (N+1xN+1) formed from an orthonormal rotation matrixR
(NxN) and a translation vectort
(Nx1).If
R
is 2x2 andt
is 2x1, thenT
is 3x3If
R
is 3x3 andt
is 3x1, thenT
is 4x4
- Seealso
rt2m, tr2rt, r2t
-
spatialmath.base.transformsNd.
rt2m
(R, t, check=False)[source]¶ Pack rotation and translation to matrix
- Parameters
R – rotation matrix
t – translation vector
check – check if rotation matrix is valid (default False, no check)
- Returns
homogeneous transform
- Return type
numpy.ndarray, shape=(3,3) or (4,4)
T = rt2m(R, t)
is a matrix (N+1xN+1) formed from a matrixR
(NxN) and a vectort
(Nx1). The bottom row is all zeros.If
R
is 2x2 andt
is 2x1, thenT
is 3x3If
R
is 3x3 andt
is 3x1, thenT
is 4x4
- Seealso
rt2tr, tr2rt, r2t
-
spatialmath.base.transformsNd.
isR
(R, tol=100)[source]¶ Test if matrix belongs to SO(n)
- Parameters
R (numpy.ndarray) – matrix to test
tol (float) – tolerance in units of eps
- Returns
whether matrix is a proper orthonormal rotation matrix
- Return type
bool
Checks orthogonality, ie. \({\bf R} {\bf R}^T = {\bf I}\) and \(\det({\bf R}) > 0\). For the first test we check that the norm of the residual is less than
tol * eps
.- Seealso
isrot2, isrot
-
spatialmath.base.transformsNd.
isskew
(S, tol=10)[source]¶ Test if matrix belongs to so(n)
- Parameters
S (numpy.ndarray) – matrix to test
tol (float) – tolerance in units of eps
- Returns
whether matrix is a proper skew-symmetric matrix
- Return type
bool
Checks skew-symmetry, ie. \({\bf S} + {\bf S}^T = {\bf 0}\). We check that the norm of the residual is less than
tol * eps
.- Seealso
isskewa
-
spatialmath.base.transformsNd.
isskewa
(S, tol=10)[source]¶ Test if matrix belongs to se(n)
- Parameters
S (numpy.ndarray) – matrix to test
tol (float) – tolerance in units of eps
- Returns
whether matrix is a proper skew-symmetric matrix
- Return type
bool
Check if matrix is augmented skew-symmetric, ie. the top left (n-1xn-1) partition
S
is skew-symmetric \({\bf S} + {\bf S}^T = {\bf 0}\), and the bottom row is zero We check that the norm of the residual is less thantol * eps
.- Seealso
isskew
-
spatialmath.base.transformsNd.
iseye
(S, tol=10)[source]¶ Test if matrix is identity
- Parameters
S (numpy.ndarray) – matrix to test
tol (float) – tolerance in units of eps
- Returns
whether matrix is a proper skew-symmetric matrix
- Return type
bool
Check if matrix is an identity matrix. We test that the trace tom row is zero We check that the norm of the residual is less than
tol * eps
.- Seealso
isskew, isskewa
-
spatialmath.base.transformsNd.
skew
(v)[source]¶ Create skew-symmetric metrix from vector
- Parameters
v (array_like) – 1- or 3-vector
- Returns
skew-symmetric matrix in so(2) or so(3)
- Return type
numpy.ndarray, shape=(2,2) or (3,3)
- Raises
ValueError
skew(V)
is a skew-symmetric matrix formed from the elements ofV
.len(V)
is 1 thenS
= \(\left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]\)len(V)
is 3 thenS
= \(\left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{array} \right]\)
Notes:
This is the inverse of the function
vex()
.These are the generator matrices for the Lie algebras so(2) and so(3).
- Seealso
vex, skewa
-
spatialmath.base.transformsNd.
vex
(s)[source]¶ Convert skew-symmetric matrix to vector
- Parameters
s (numpy.ndarray, shape=(2,2) or (3,3)) – skew-symmetric matrix
- Returns
vector of unique values
- Return type
numpy.ndarray, shape=(1,) or (3,)
- Raises
ValueError
vex(S)
is the vector which has the corresponding skew-symmetric matrixS
.S
is 2x2 - so(2) case - whereS
\(= \left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]\) then return \([v]\)S
is 3x3 - so(3) case - whereS
\(= \left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{array} \right]\) then return \([v_x, v_y, v_z]\).
Notes:
This is the inverse of the function
skew()
.Only rudimentary checking (zero diagonal) is done to ensure that the matrix is actually skew-symmetric.
The function takes the mean of the two elements that correspond to each unique element of the matrix.
- Seealso
skew, vexa
-
spatialmath.base.transformsNd.
skewa
(v)[source]¶ Create augmented skew-symmetric metrix from vector
- Parameters
v (array_like) – 3- or 6-vector
- Returns
augmented skew-symmetric matrix in se(2) or se(3)
- Return type
numpy.ndarray, shape=(3,3) or (4,4)
- Raises
ValueError
skewa(V)
is an augmented skew-symmetric matrix formed from the elements ofV
.len(V)
is 3 then S = \(\left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]\)len(V)
is 6 then S = \(\left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]\)
Notes:
This is the inverse of the function
vexa()
.These are the generator matrices for the Lie algebras se(2) and se(3).
Map twist vectors in 2D and 3D space to se(2) and se(3).
- Seealso
vexa, skew
-
spatialmath.base.transformsNd.
vexa
(Omega)[source]¶ Convert skew-symmetric matrix to vector
- Parameters
s (numpy.ndarray, shape=(3,3) or (4,4)) – augmented skew-symmetric matrix
- Returns
vector of unique values
- Return type
numpy.ndarray, shape=(3,) or (6,)
- Raises
ValueError
vex(S)
is the vector which has the corresponding skew-symmetric matrixS
.S
is 3x3 - se(2) case - whereS
\(= \left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]\) then return \([v_1, v_2, v_3]\).S
is 4x4 - se(3) case - whereS
\(= \left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]\) then return \([v_1, v_2, v_3, v_4, v_5, v_6]\).
Notes:
This is the inverse of the function
skewa
.Only rudimentary checking (zero diagonal) is done to ensure that the matrix is actually skew-symmetric.
The function takes the mean of the two elements that correspond to each unique element of the matrix.
- Seealso
skewa, vex
-
spatialmath.base.transformsNd.
h2e
(v)[source]¶ Convert from homogeneous to Euclidean form
- Parameters
v (array_like) – homogeneous vector or matrix
- Returns
Euclidean vector
- Return type
numpy.ndarray
If
v
is an array, shape=(N,), return an array shape=(N-1,) where the elements have all been scaled by the last element ofv
.If
v
is a matrix, shape=(N,M), return a matrix shape=(N-1,N), where each column has been scaled by its last element.
- Seealso
e2h
-
spatialmath.base.transformsNd.
e2h
(v)[source]¶ Convert from Euclidean to homogeneous form
- Parameters
v (array_like) – Euclidean vector or matrix
- Returns
homogeneous vector
- Return type
numpy.ndarray
If
v
is an array, shape=(N,), return an array shape=(N+1,) where a value of 1 has been appendedIf
v
is a matrix, shape=(N,M), return a matrix shape=(N+1,N), where each column has been appended with a value of 1, ie. a row of ones has been appended to the matrix.
- Seealso
e2h
Vectors¶
This modules contains functions to create and transform rotation matrices and homogeneous tranformation matrices.
Vector arguments are what numpy refers to as array_like
and can be a list,
tuple, numpy array, numpy row vector or numpy column vector.
-
spatialmath.base.vectors.
unitvec
(v)[source]¶ Create a unit vector
- Parameters
v (array_like) – n-dimensional vector
- Returns
a unit-vector parallel to V.
- Return type
numpy.ndarray
- Raises
ValueError – for zero length vector
unitvec(v)
is a vector parallel to v of unit length.- Seealso
norm
-
spatialmath.base.vectors.
norm
(v)[source]¶ Norm of vector
- Parameters
v – n-vector as a list, dict, or a numpy array, row or column vector
- Returns
norm of vector
- Return type
float
norm(v)
is the 2-norm (length or magnitude) of the vectorv
.- Seealso
unit
-
spatialmath.base.vectors.
isunitvec
(v, tol=10)[source]¶ Test if vector has unit length
- Parameters
v (numpy.ndarray) – vector to test
tol (float) – tolerance in units of eps
- Returns
whether vector has unit length
- Return type
bool
- Seealso
unit, isunittwist
-
spatialmath.base.vectors.
iszerovec
(v, tol=10)[source]¶ Test if vector has zero length
- Parameters
v (numpy.ndarray) – vector to test
tol (float) – tolerance in units of eps
- Returns
whether vector has zero length
- Return type
bool
- Seealso
unit, isunittwist
-
spatialmath.base.vectors.
isunittwist
(v, tol=10)[source]¶ Test if vector represents a unit twist in SE(2) or SE(3)
- Parameters
v (array_like) – vector to test
tol (float) – tolerance in units of eps
- Returns
whether vector has unit length
- Return type
bool
Vector is is intepretted as \([v, \omega]\) where \(v \in \mathbb{R}^n\) and \(\omega \in \mathbb{R}^1\) for SE(2) and \(\omega \in \mathbb{R}^3\) for SE(3).
A unit twist can be a:
unit rotational twist where \(|| \omega || = 1\), or
unit translational twist where \(|| \omega || = 0\) and \(|| v || = 1\).
- Seealso
unit, isunitvec
-
spatialmath.base.vectors.
isunittwist2
(v, tol=10)[source]¶ Test if vector represents a unit twist in SE(2) or SE(3)
- Parameters
v (array_like) – vector to test
tol (float) – tolerance in units of eps
- Returns
whether vector has unit length
- Return type
bool
Vector is is intepretted as \([v, \omega]\) where \(v \in \mathbb{R}^n\) and \(\omega \in \mathbb{R}^1\) for SE(2) and \(\omega \in \mathbb{R}^3\) for SE(3).
A unit twist can be a:
unit rotational twist where \(|| \omega || = 1\), or
unit translational twist where \(|| \omega || = 0\) and \(|| v || = 1\).
- Seealso
unit, isunitvec
-
spatialmath.base.vectors.
unittwist
(S, tol=10)[source]¶ Convert twist to unit twist
- Parameters
S (array_like) – twist as a 6-vector
tol (float) – tolerance in units of eps
- Returns
unit twist and scalar motion
- Return type
np.ndarray, shape=(6,)
A unit twist is a twist where:
the rotation part has unit magnitude
if the rotational part is zero, then the translational part has unit magnitude
Returns None if the twist has zero magnitude
-
spatialmath.base.vectors.
unittwist_norm
(S, tol=10)[source]¶ Convert twist to unit twist and norm
- Parameters
S (array_like) – twist as a 6-vector
tol (float) – tolerance in units of eps
- Returns
unit twist and scalar motion
- Return type
tuple (np.ndarray shape=(6,), theta)
A unit twist is a twist where:
the rotation part has unit magnitude
if the rotational part is zero, then the translational part has unit magnitude
Returns (None,None) if the twist has zero magnitude
-
spatialmath.base.vectors.
unittwist2
(S)[source]¶ Convert twist to unit twist
- Parameters
S (array_like) – twist as a 3-vector
- Returns
unit twist and scalar motion
- Return type
tuple (unit_twist, theta)
A unit twist is a twist where:
the rotation part has unit magnitude
if the rotational part is zero, then the translational part has unit magnitude
-
spatialmath.base.vectors.
angdiff
(a, b)[source]¶ Angular difference
- Parameters
a (scalar or array_like) – angle in radians
b (scalar or array_like) – angle in radians
- Returns
angular difference a-b
- Return type
scalar or array_like
If
a
andb
are both scalars, the result is scalarIf
a
is array_like, the result is a vector a[i]-bIf
a
is array_like, the result is a vector a-b[i]If
a
andb
are both vectors of the same length, the result is a vector a[i]-b[i]
Quaternions¶
Created on Fri Apr 10 14:12:56 2020
@author: Peter Corke
-
spatialmath.base.quaternions.
eye
()[source]¶ Create an identity quaternion
- Returns
an identity quaternion
- Return type
numpy.ndarray, shape=(4,)
Creates an identity quaternion, with the scalar part equal to one, and a zero vector value.
-
spatialmath.base.quaternions.
pure
(v)[source]¶ Create a pure quaternion
- Parameters
v (array_like) – vector from a 3-vector
- Returns
pure quaternion
- Return type
numpy.ndarray, shape=(4,)
Creates a pure quaternion, with a zero scalar value and the vector part equal to the passed vector value.
-
spatialmath.base.quaternions.
qnorm
(q)[source]¶ Norm of a quaternion
- Parameters
q – input quaternion as a 4-vector
- Returns
norm of the quaternion
- Return type
float
Returns the norm, length or magnitude of the input quaternion which is \(\sqrt{s^2 + v_x^2 + v_y^2 + v_z^2}\)
- Seealso
unit
-
spatialmath.base.quaternions.
unit
(q, tol=10)[source]¶ Create a unit quaternion
- Parameters
v (array_like) – quaterion as a 4-vector
- Returns
a pure quaternion
- Return type
numpy.ndarray, shape=(4,)
Creates a unit quaternion, with unit norm, by scaling the input quaternion.
See also
norm
-
spatialmath.base.quaternions.
isunit
(q, tol=10)[source]¶ Test if quaternion has unit length
- Parameters
v (array_like) – quaternion as a 4-vector
tol (float) – tolerance in units of eps
- Returns
whether quaternion has unit length
- Return type
bool
- Seealso
unit
-
spatialmath.base.quaternions.
isequal
(q1, q2, tol=100, unitq=False)[source]¶ Test if quaternions are equal
- Parameters
q1 (array_like) – quaternion as a 4-vector
q2 (array_like) – quaternion as a 4-vector
unitq (bool) – quaternions are unit quaternions
tol (float) – tolerance in units of eps
- Returns
whether quaternion has unit length
- Return type
bool
Tests if two quaternions are equal.
For unit-quaternions
unitq=True
the double mapping is taken into account, that isq
and-q
represent the same orientation andisequal(q, -q, unitq=True)
will returnTrue
.
-
spatialmath.base.quaternions.
q2v
(q)[source]¶ Convert unit-quaternion to 3-vector
- Parameters
q – unit-quaternion as a 4-vector
- Returns
a unique 3-vector
- Return type
numpy.ndarray, shape=(3,)
Returns a unique 3-vector representing the input unit-quaternion. The sign of the scalar part is made positive, if necessary by multiplying the entire quaternion by -1, then the vector part is taken.
Warning
There is no check that the passed value is a unit-quaternion.
See also
v2q
-
spatialmath.base.quaternions.
v2q
(v)[source]¶ Convert 3-vector to unit-quaternion
- Parameters
v (array_like) – vector part of unit quaternion, a 3-vector
- Returns
a unit quaternion
- Return type
numpy.ndarray, shape=(4,)
Returns a unit-quaternion reconsituted from just its vector part. Assumes that the scalar part was positive, so \(s = \sqrt{1-||v||}\).
See also
q2v
-
spatialmath.base.quaternions.
qqmul
(q1, q2)[source]¶ Quaternion multiplication
- Parameters
q0 (: array_like) – left-hand quaternion as a 4-vector
q1 (array_like) – right-hand quaternion as a 4-vector
- Returns
quaternion product
- Return type
numpy.ndarray, shape=(4,)
This is the quaternion or Hamilton product. If both operands are unit-quaternions then the product will be a unit-quaternion.
- Seealso
qvmul, inner, vvmul
-
spatialmath.base.quaternions.
inner
(q1, q2)[source]¶ Quaternion innert product
- Parameters
q0 (: array_like) – quaternion as a 4-vector
q1 (array_like) – uaternion as a 4-vector
- Returns
inner product
- Return type
numpy.ndarray, shape=(4,)
This is the inner or dot product of two quaternions, it is the sum of the element-wise product.
- Seealso
qvmul
-
spatialmath.base.quaternions.
qvmul
(q, v)[source]¶ Vector rotation
- Parameters
q (array_like) – unit-quaternion as a 4-vector
v (list, tuple, numpy.ndarray) – 3-vector to be rotated
- Returns
rotated 3-vector
- Return type
numpy.ndarray, shape=(3,)
The vector v is rotated about the origin by the SO(3) equivalent of the unit quaternion.
Warning
There is no check that the passed value is a unit-quaternions.
- Seealso
qvmul
-
spatialmath.base.quaternions.
vvmul
(qa, qb)[source]¶ Quaternion multiplication
- Parameters
qa (: array_like) – left-hand quaternion as a 3-vector
qb (array_like) – right-hand quaternion as a 3-vector
- Returns
quaternion product
- Return type
numpy.ndarray, shape=(3,)
This is the quaternion or Hamilton product of unit-quaternions defined only by their vector components. The product will be a unit-quaternion, defined only by its vector component.
- Seealso
qvmul, inner
-
spatialmath.base.quaternions.
pow
(q, power)[source]¶ Raise quaternion to a power
- Parameters
q – quaternion as a 4-vector
power (int) – exponent
- Returns
input quaternion raised to the specified power
- Return type
numpy.ndarray, shape=(4,)
Raises a quaternion to the specified power using repeated multiplication.
Notes:
power must be an integer
power can be negative, in which case the conjugate is taken
-
spatialmath.base.quaternions.
conj
(q)[source]¶ Quaternion conjugate
- Parameters
q – quaternion as a 4-vector
- Returns
conjugate of input quaternion
- Return type
numpy.ndarray, shape=(4,)
Conjugate of quaternion, the vector part is negated.
-
spatialmath.base.quaternions.
q2r
(q)[source]¶ Convert unit-quaternion to SO(3) rotation matrix
- Parameters
q – unit-quaternion as a 4-vector
- Returns
corresponding SO(3) rotation matrix
- Return type
numpy.ndarray, shape=(3,3)
Returns an SO(3) rotation matrix corresponding to this unit-quaternion.
Warning
There is no check that the passed value is a unit-quaternion.
- Seealso
r2q
-
spatialmath.base.quaternions.
r2q
(R, check=True)[source]¶ Convert SO(3) rotation matrix to unit-quaternion
- Parameters
R (numpy.ndarray, shape=(3,3)) – rotation matrix
- Returns
unit-quaternion
- Return type
numpy.ndarray, shape=(3,)
Returns a unit-quaternion corresponding to the input SO(3) rotation matrix.
Warning
There is no check that the passed matrix is a valid rotation matrix.
- Seealso
q2r
-
spatialmath.base.quaternions.
slerp
(q0, q1, s, shortest=False)[source]¶ Quaternion conjugate
- Parameters
q0 (array_like) – initial unit quaternion as a 4-vector
q1 (array_like) – final unit quaternion as a 4-vector
s (float) – interpolation coefficient in the range [0,1]
shortest (bool) – choose shortest distance [default False]
- Returns
interpolated unit-quaternion
- Return type
numpy.ndarray, shape=(4,)
An interpolated quaternion between
q0
whens
= 0 toq1
whens
= 1.Interpolation is performed on a great circle on a 4D hypersphere. This is a rotation about a single fixed axis in space which yields the straightest and shortest path between two points.
For large rotations the path may be the long way around the circle, the option
'shortest'
ensures always the shortest path.Warning
There is no check that the passed values are unit-quaternions.
-
spatialmath.base.quaternions.
rand
()[source]¶ Random unit-quaternion
- Returns
random unit-quaternion
- Return type
numpy.ndarray, shape=(4,)
Computes a uniformly distributed random unit-quaternion which can be considered equivalent to a random SO(3) rotation.
-
spatialmath.base.quaternions.
matrix
(q)[source]¶ Convert to 4x4 matrix equivalent
- Parameters
q – quaternion as a 4-vector
- Returns
equivalent matrix
- Return type
numpy.ndarray, shape=(4,4)
Hamilton multiplication between two quaternions can be considered as a matrix-vector product, the left-hand quaternion is represented by an equivalent 4x4 matrix and the right-hand quaternion as 4x1 column vector.
- Seealso
qqmul
-
spatialmath.base.quaternions.
dot
(q, w)[source]¶ Rate of change of unit-quaternion
- Parameters
q0 (array_like) – unit-quaternion as a 4-vector
w (array_like) – angular velocity in world frame as a 3-vector
- Returns
rate of change of unit quaternion
- Return type
numpy.ndarray, shape=(4,)
dot(q, w)
is the rate of change of the elements of the unit quaternionq
which represents the orientation of a body frame with angular velocityw
in the world frame.Warning
There is no check that the passed values are unit-quaternions.
-
spatialmath.base.quaternions.
dotb
(q, w)[source]¶ Rate of change of unit-quaternion
- Parameters
q0 (array_like) – unit-quaternion as a 4-vector
w (array_like) – angular velocity in body frame as a 3-vector
- Returns
rate of change of unit quaternion
- Return type
numpy.ndarray, shape=(4,)
dot(q, w)
is the rate of change of the elements of the unit quaternionq
which represents the orientation of a body frame with angular velocityw
in the body frame.Warning
There is no check that the passed values are unit-quaternions.
-
spatialmath.base.quaternions.
angle
(q1, q2)[source]¶ Angle between two unit-quaternions
- Parameters
q0 (array_like) – unit-quaternion as a 4-vector
q1 (array_like) – unit-quaternion as a 4-vector
- Returns
angle between the rotations [radians]
- Return type
float
If each of the input quaternions is considered a rotated coordinate frame, then the angle is the smallest rotation required about a fixed axis, to rotate the first frame into the second.
References: Metrics for 3D rotations: comparison and analysis, Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2.
Warning
There is no check that the passed values are unit-quaternions.
-
spatialmath.base.quaternions.
qprint
(q, delim=('<', '>'), fmt='%f', file=<_io.TextIOWrapper name='<stdout>' mode='w' encoding='UTF-8'>)[source]¶ Format a quaternion
- Parameters
q (array_like) – unit-quaternion as a 4-vector
delim (list or tuple of strings) – 2-list of delimeters [default (‘<’, ‘>’)]
fmt (str) – printf-style format soecifier [default ‘%f’]
file (file object) – destination for formatted string [default sys.stdout]
- Returns
formatted string
- Return type
str
Format the quaternion in a human-readable form as:
S D1 VX VY VZ D2
where S, VX, VY, VZ are the quaternion elements, and D1 and D2 are a pair of delimeters given by delim.
By default the string is written to sys.stdout.
If file=None then a string is returned.