Vectors

Functions to manipulate vectors

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.

angdiff(a, b=None)[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

angdiff(a, b) is the difference a - b wrapped to the range \([-\pi, \pi)\). This is the operator \(a \circleddash b\) used in the RVC book
- If a and b are both scalars, the result is scalar
- If a is array_like, the result is a NumPy array a[i]-b
- If a is array_like, the result is a NumPy array a-b[i]
- If a and b are both vectors of the same length, the result is a NumPy array a[i]-b[i]
angdiff(a) is the angle or vector of angles a wrapped to the range \([-\pi, \pi)\).
- If a is a scalar, the result is scalar
- If a is array_like, the result is a NumPy array

>>> from spatialmath.base import *
>>> from math import pi
>>> angdiff(0, 2 * pi)
0.0
>>> angdiff(0.9 * pi, -0.9 * pi) / pi
-0.2
>>> angdiff(3 * pi)
-3.141592653589793

Seealso: vector_diff() wrap_mpi_pi()

angle_mean(theta)[source]

Mean of angular values

Parameters: theta (Union[float, List[float], Tuple[float, ...], ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]) – angular values
Returns: circular mean
Return type: float

The circular mean is given by

\[\bar{\theta} = \tan^{-1} \frac{\sum \sin \theta_i}{\sum \cos \theta_i} \in [-\pi, \pi)]\]

Seealso: angle_std()

angle_std(theta)[source]

Standard deviation of angular values

Parameters: theta (array_like) – angular values
Returns: circular standard deviation
Return type: float

\[\sigma_{\theta} = \sqrt{-2 \log \| \left[ \frac{\sum \sin \theta_i}{N}, \frac{\sum \sin \theta_i}{N} \right] \|} \in [0, \infty)\]

Seealso: angle_mean()

angle_wrap(theta, mode='-pi:pi')[source]

Generalized angle-wrapping

Parameters

v (array_like) – angles to wrap
mode (str, optional) – wrapping mode, one of: "0:2pi", "0:pi", "-pi/2:pi/2" or "-pi:pi" [default]

Returns

wrapped angles

Return type

ndarray

Note

The modes "0:pi" and "-pi/2:pi/2" are used to wrap angles of colatitude and latitude respectively.

Seealso: wrap_0_2pi() wrap_mpi_pi() wrap_0_pi() wrap_mpi2_pi2()

colvec(v)[source]

Create a column vector

Parameters: v (array_like(n)) – any vector
Returns: a column vector
Return type: ndarray(n,1)

Convert input to a column vector.

>>> from spatialmath.base import *
>>> colvec([1, 2, 3])
array([[1.],
       [2.],
       [3.]])

cross(u, v)[source]

Cross product of vectors

Parameters

u (array_like(3)) – any vector
v (array_like(3)) – any vector

Returns

cross product

Return type

nd.array(3)

cross(u, v) is the cross product of the vectors u and v.

>>> from spatialmath.base import *
>>> cross([1, 0, 0], [0, 1, 0])
array([0, 0, 1])

Note

This function does not use NumPy, it is ~1.5x faster than numpy.cross()

Seealso: unit()
SymPy: supported

isunittwist(v, tol=20)[source]

Test if vector represents a unit twist in SE(2) or SE(3)

Parameters

v (array_like(6)) – twist vector to test
tol (float) – tolerance in units of eps, defaults to 20

Returns

whether twist has unit length

Return type

bool

Raises

ValueError – for incorrect vector length

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\).

>>> from spatialmath.base import *
>>> isunittwist([1, 2, 3, 1, 0, 0])
True
>>> isunittwist([0, 0, 0, 2, 0, 0])
False

Seealso: unit, isunitvec

isunittwist2(v, tol=20)[source]

Test if vector represents a unit twist in SE(2) or SE(3)

Parameters

v (array_like(3)) – twist vector to test
tol (float) – tolerance in units of eps, defaults to 20

Returns

whether vector has unit length

Return type

bool

Raises

ValueError – for incorrect vector length

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\).

>>> from spatialmath.base import *
>>> isunittwist2([1, 2, 1])
True
>>> isunittwist2([0, 0, 2])
False

Seealso: unit, isunitvec

isunitvec(v, tol=20)[source]

Test if vector has unit length

Parameters

v (ndarray(n)) – vector to test
tol (float) – tolerance in units of eps, defaults to 20

Returns

whether vector has unit length

Return type

bool

>>> from spatialmath.base import *
>>> isunitvec([1, 0])
True
>>> isunitvec([1, 2])
False

Seealso: unit, iszerovec, isunittwist

iszero(v, tol=20)[source]

Test if scalar is zero

Parameters

v (float) – value to test
tol (float) – tolerance in units of eps, defaults to 20

Returns

whether value is zero

Return type

bool

>>> from spatialmath.base import *
>>> iszero(0)
True
>>> iszero(1)
False

Seealso: unit, iszerovec, isunittwist

iszerovec(v, tol=20)[source]

Test if vector has zero length

Parameters

v (ndarray(n)) – vector to test
tol (float) – tolerance in units of eps, defaults to 20

Returns

whether vector has zero length

Return type

bool

>>> from spatialmath.base import *
>>> iszerovec([0, 0])
True
>>> iszerovec([1, 2])
False

Seealso: unit, isunitvec, isunittwist

norm(v)[source]

Norm of vector

Parameters: v (array_like(n)) – any vector
Returns: norm of vector
Return type: float

norm(v) is the 2-norm (length or magnitude) of the vector v.

>>> from spatialmath.base import *
>>> norm([3, 4])
5.0

Note

This function does not use NumPy, it is ~2x faster than numpy.linalg.norm() for a 3-vector

Seealso: unit()
SymPy: supported

normsq(v)[source]

Squared norm of vector

Parameters: v (array_like(n)) – any vector
Returns: norm of vector
Return type: float

norm(sq) is the sum of squared elements of the vector v or \(|v|^2\).

>>> from spatialmath.base import *
>>> normsq([2, 3])
13

Note

This function does not use NumPy, it is ~2x faster than numpy.linalg.norm() ** 2 for a 3-vector

Seealso: unit()
SymPy: supported

orthogonalize(v1, v2, normalize=True)[source]

Orthoginalizes vector v1 with respect to v2 with minimum rotation. Returns a the nearest vector to v1 that is orthoginal to v2.

Parameters

v1 (array_like(n)) – vector to be orthoginalized
v2 (array_like(n)) – vector that returned vector will be orthoginal to
normalize (bool) – whether to normalize the output vector

Returns

nearest vector to v1 that is orthoginal to v2

Return type

ndarray(n)

project(v1, v2)[source]

Projects vector v1 onto v2. Returns a vector parallel to v2.

Parameters

v1 (array_like(n)) – vector to be projected
v2 (array_like(n)) – vector to be projected onto

Returns

vector projection of v1 onto v2 (parrallel to v2)

Return type

ndarray(n)

removesmall(v, tol=20)[source]

Set small values to zero

Parameters

v (array_like(n) or ndarray(n,m)) – any vector
tol (int, optional) – Tolerance in units of eps, defaults to 20

Returns

vector with small values set to zero

Return type

ndarray(n) or ndarray(n,m)

Values with absolute value less than tol will be set to zero.

>>> from spatialmath.base import *
>>> a = np.r_[1, 2, 3, 1e-16]
>>> print(a)
[1. 2. 3. 0.]
>>> a = removesmall(a)
>>> print(a)
[1. 2. 3. 0.]
>>> print(a[3])
0.0

unittwist(S, tol=20)[source]

Convert twist to unit twist

Parameters

S (array_like(6)) – twist vector
tol (float) – tolerance in units of eps, defaults to 20

Returns

unit twist

Return type

ndarray(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

>>> from spatialmath.base import *
>>> unittwist([2, 4, 6, 2, 0, 0])
array([1., 2., 3., 1., 0., 0.])
>>> unittwist([2, 0, 0, 0, 0, 0])
array([1., 0., 0., 0., 0., 0.])

Returns None if the twist has zero magnitude

unittwist2(S, tol=20)[source]

Convert twist to unit twist

Parameters

S (array_like(3)) – twist vector
tol (float) – tolerance in units of eps, defaults to 20

Returns

unit twist

Return type

ndarray(3)

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

Note

Returns None if the twist has zero magnitude

unittwist2_norm(S, tol=20)[source]

Convert twist to unit twist

Parameters

S (array_like(3)) – twist vector
tol (float) – tolerance in units of eps, defaults to 20

Returns

unit twist and scalar motion

Return type

tuple (ndarray(3), float)

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

Note

Returns (None, None) if the twist has zero magnitude

unittwist_norm(S, tol=20)[source]

Convert twist to unit twist and norm

Parameters

S (array_like(6)) – twist vector
tol (float) – tolerance in units of eps, defaults to 20

Returns

unit twist and scalar motion

Return type

tuple (ndarray(6), float)

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

>>> from spatialmath.base import *
>>> S, n = unittwist_norm([1, 2, 3, 1, 0, 0])
>>> print(S, n)
[1. 2. 3. 1. 0. 0.] 1.0
>>> S, n = unittwist_norm([0, 0, 0, 2, 0, 0])
>>> print(S, n)
[0. 0. 0. 1. 0. 0.] 2.0
>>> S, n = unittwist_norm([0, 0, 0, 0, 0, 0])
>>> print(S, n)
None None

Note

Returns (None,None) if the twist has zero magnitude

unitvec(v, tol=20)[source]

Create a unit vector

Parameters

v (array_like(n)) – any vector
tol (float) – Tolerance in units of eps for zero-norm case, defaults to 20

Type

float

Returns

a unit-vector parallel to v.

Return type

ndarray(n)

Raises

ValueError – for zero length vector

unitvec(v) is a vector parallel to v of unit length.

>>> from spatialmath.base import *
>>> unitvec([3, 4])
array([0.6, 0.8])

Seealso: norm()

unitvec_norm(v, tol=20)[source]

Create a unit vector

Parameters

v (array_like(n)) – any vector
tol (float) – Tolerance in units of eps for zero-norm case, defaults to 20

Type

float

Returns

a unit-vector parallel to v and the norm

Return type

(ndarray(n), float)

Raises

ValueError – for zero length vector

unitvec(v) is a vector parallel to v of unit length.

>>> from spatialmath.base import *
>>> unitvec([3, 4])
array([0.6, 0.8])

Seealso: norm()

vector_diff(v1, v2, mode)[source]

Generalized vector differnce

Parameters

v1 (array_like(n)) – first vector
v2 (array_like(n)) – second vector
mode (str of length n) – subtraction mode

mode character	purpose
r	real number, don’t wrap
c	angle on circle, wrap to [-π, π)
C	angle on circle, wrap to [0, 2π)
l	latitude angle, wrap to [-π/2, π/2]
L	colatitude angle, wrap to [0, π]

Seealso: angdiff() wrap_0_2pi() wrap_mpi_pi() wrap_0_pi() wrap_mpi2_pi2()
Return type: ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]

wrap_0_2pi(theta)[source]

Wrap angle to range \([0, 2\pi)\)

Parameters: theta (scalar or ndarray) – input angle
Return type: Union[float, ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]
Returns: angle wrapped into range \([0, 2\pi)\)
Seealso: wrap_mpi_pi() wrap_0_pi() wrap_mpi2_pi2() angle_wrap()

wrap_0_pi(theta)[source]

Wrap angle to range \([0, \pi]\)

Parameters: theta (scalar or ndarray) – input angle
Return type: Union[float, ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]
Returns: angle wrapped into range \([0, \pi)\)

This is used to fold angles of colatitude. If zero is the angle of the north pole, colatitude increases to \(\pi\) at the south pole then decreases to \(0\) as we head back to the north pole.

Seealso: wrap_mpi2_pi2() wrap_0_2pi() wrap_mpi_pi() angle_wrap()

wrap_mpi2_pi2(theta)[source]

Wrap angle to range \([-\pi/2, \pi/2]\)

Parameters: theta (scalar or ndarray) – input angle
Return type: Union[float, ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]
Returns: angle wrapped into range \([-\pi/2, \pi/2]\)

This is used to fold angles of latitude.

Seealso: wrap_0_pi() wrap_0_2pi() wrap_mpi_pi() angle_wrap()

wrap_mpi_pi(theta)[source]

Wrap angle to range \([-\pi, \pi)\)

Parameters: theta (scalar or ndarray) – input angle
Return type: Union[float, ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]
Returns: angle wrapped into range \([-\pi, \pi)\)
Seealso: wrap_0_2pi() wrap_0_pi() wrap_mpi2_pi2() angle_wrap()