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)

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)

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)

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')

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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)

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

Parameters:

theta (scalar or ndarray) – input angle

Returns:

angle wrapped into range \([0, 2\pi)\)

Return type:

scalar or ndarray

Seealso:

wrap_mpi_pi() wrap_0_pi() wrap_mpi2_pi2() angle_wrap()

wrap_0_pi(theta)

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

Parameters:

theta (scalar or ndarray) – input angle

Returns:

angle wrapped into range \([0, \pi)\)

Return type:

scalar or ndarray

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)

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

Parameters:

theta (scalar or ndarray) – input angle

Returns:

angle wrapped into range \([-\pi/2, \pi/2]\)

Return type:

scalar or ndarray

This is used to fold angles of latitude.

Seealso:

wrap_0_pi() wrap_0_2pi() wrap_mpi_pi() angle_wrap()

wrap_mpi_pi(theta)

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

Parameters:

theta (scalar or ndarray) – input angle

Returns:

angle wrapped into range \([-\pi, \pi)\)

Return type:

scalar or ndarray

Seealso:

wrap_0_2pi() wrap_0_pi() wrap_mpi2_pi2() angle_wrap()