Chapter 3: Rigid-Body Motions

Functions
const arma::mat33	mr::RotInv (const arma::mat33 &R)
	Inverts a rotation matrix. More...
const arma::mat33	mr::VecToso3 (const arma::vec3 &omg)
	Converts a 3-vector to an so(3) representation. More...
const arma::vec3	mr::so3ToVec (const arma::mat33 &so3mat)
	Converts an so(3) representation to a 3-vector. More...
const std::tuple< const arma::vec3, double >	mr::AxisAng3 (const arma::vec3 &expc3)
	Converts a 3-vector of exponential coordinates for rotation into axis-angle form. More...
const arma::mat33	mr::MatrixExp3 (const arma::mat33 &so3mat)
	Computes the matrix exponential of a matrix in so(3) More...
const arma::mat33	mr::MatrixLog3 (const arma::mat33 &R)
	Computes the matrix logarithm of a rotation matrix. More...
const arma::mat44	mr::RpToTrans (const arma::mat33 &R, const arma::vec3 &p)
	Converts a rotation matrix and a position vector into homogeneous transformation matrix. More...
const std::tuple< const arma::mat33, const arma::vec3 >	mr::TransToRp (const arma::mat44 &T)
	Converts a homogeneous transformation matrix into a rotation matrix and position vector. More...
const arma::mat44	mr::TransInv (const arma::mat44 &T)
	Inverts a homogeneous transformation matrix. More...
const arma::mat44	mr::VecTose3 (const arma::vec6 &V)
	Converts a spatial velocity vector into a 4x4 matrix in se3. More...
const arma::vec6	mr::se3ToVec (const arma::mat44 &se3mat)
	Converts an se3 matrix into a spatial velocity vector. More...
const arma::mat66	mr::Adjoint (const arma::mat44 &T)
	Computes the adjoint representation of a homogeneous transformation matrix. More...
const arma::vec6	mr::ScrewToAxis (const arma::vec3 &q, const arma::vec3 &s, const double h)
	Takes a parametric description of a screw axis and converts it to a normalized screw axis. More...
const std::tuple< const arma::vec6, double >	mr::AxisAng6 (const arma::vec6 &expc6)
	Converts a 6-vector of exponential coordinates into screw axis-angle form. More...
const arma::mat44	mr::MatrixExp6 (const arma::mat44 &se3mat)
	Computes the matrix exponential of an se3 representation of exponential coordinates. More...
const arma::mat44	mr::MatrixLog6 (const arma::mat44 &T)
	Computes the matrix logarithm of a homogeneous transformation matrix. More...
const arma::mat33	mr::ProjectToSO3 (const arma::mat33 &mat)
	Returns a projection of mat into SO(3) More...
const arma::mat44	mr::ProjectToSE3 (const arma::mat44 &mat)
	Returns a projection of mat into SE(3) More...
double	mr::DistanceToSO3 (const arma::mat33 &mat)
	Returns the Frobenius norm to describe the distance of mat from the SO(3) manifold. More...
double	mr::DistanceToSE3 (const arma::mat44 &mat)
	Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold. More...
bool	mr::TestIfSO3 (const arma::mat33 &mat)
	Returns true if mat is close to or on the manifold SO(3) More...
bool	mr::TestIfSE3 (const arma::mat44 &mat)
	Returns true if mat is close to or on the manifold SE(3) More...

Detailed Description

Function Documentation

Adjoint()

const arma::mat66 mr::Adjoint

(

const arma::mat44 &

T

)

Computes the adjoint representation of a homogeneous transformation matrix.

Parameters

T	A homogeneous transformation matrix

Returns

The 6x6 adjoint representation [AdT] of T

AxisAng3()

const std::tuple< const arma::vec3, double > mr::AxisAng3

(

const arma::vec3 &

expc3

)

Converts a 3-vector of exponential coordinates for rotation into axis-angle form.

Parameters

expc3

A 3-vector of exponential coordinates for rotation

Returns

omghat: A unit rotation axis

theta: The corresponding rotation angle

AxisAng6()

const std::tuple< const arma::vec6, double > mr::AxisAng6

(

const arma::vec6 &

expc6

)

Converts a 6-vector of exponential coordinates into screw axis-angle form.

Parameters

expc6

A 6-vector of exponential coordinates for rigid-body motion S*theta

Returns

S: The corresponding normalized screw axis

theta: The distance traveled along/about S

DistanceToSE3()

double mr::DistanceToSE3

(

const arma::mat44 &

mat

)

Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold.

Parameters

mat	A 4x4 matrix

Returns

A quantity describing the distance of mat from the SE(3) manifold

Computes the distance from mat to the SE(3) manifold using the following method: Compute the determinant of matR, the top 3x3 submatrix of mat. If det(matR) <= 0, return a large number. If det(matR) > 0, replace the top 3x3 submatrix of mat with matR^T.matR, and set the first three entries of the fourth column of mat to zero. Then return norm(mat - I).

DistanceToSO3()

double mr::DistanceToSO3

(

const arma::mat33 &

mat

)

Returns the Frobenius norm to describe the distance of mat from the SO(3) manifold.

Parameters

mat	A 3x3 matrix

Returns

A quantity describing the distance of mat from the SO(3) manifold

Computes the distance from mat to the SO(3) manifold using the following method: If det(mat) <= 0, return a large number. If det(mat) > 0, return norm(mat^T.mat - I).

MatrixExp3()

const arma::mat33 mr::MatrixExp3

(

const arma::mat33 &

so3mat

)

Computes the matrix exponential of a matrix in so(3)

Parameters

so3mat

A 3x3 skew-symmetric matrix

Returns

The matrix exponential of so3mat

MatrixExp6()

const arma::mat44 mr::MatrixExp6

(

const arma::mat44 &

se3mat

)

Computes the matrix exponential of an se3 representation of exponential coordinates.

Parameters

se3mat

A matrix in se3

Returns

The matrix exponential of se3mat

MatrixLog3()

const arma::mat33 mr::MatrixLog3

(

const arma::mat33 &

R

)

Computes the matrix logarithm of a rotation matrix.

Parameters

R	A 3x3 rotation matrix

Returns

The matrix logarithm of R

MatrixLog6()

const arma::mat44 mr::MatrixLog6

(

const arma::mat44 &

T

)

Computes the matrix logarithm of a homogeneous transformation matrix.

Parameters

T	A matrix in SE3

Returns

The matrix logarithm of R

ProjectToSE3()

const arma::mat44 mr::ProjectToSE3

(

const arma::mat44 &

mat

)

Returns a projection of mat into SE(3)

Parameters

mat	A 4x4 matrix to project to SE(3)

Returns

The closest matrix to T that is in SE(3)

Projects a matrix mat to the closest matrix in SE(3) using singular-value decomposition (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). This function is only appropriate for matrices close to SE(3).

ProjectToSO3()

const arma::mat33 mr::ProjectToSO3

(

const arma::mat33 &

mat

)

Returns a projection of mat into SO(3)

Parameters

mat	A matrix near SO(3) to project to SO(3)

Returns

The closest matrix to R that is in SO(3)

Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). This function is only appropriate for matrices close to SO(3).

RotInv()

const arma::mat33 mr::RotInv

(

const arma::mat33 &

R

)

Inverts a rotation matrix.

Parameters

R	rotation matrix

Returns

The inverse of R

RpToTrans()

const arma::mat44 mr::RpToTrans	(	const arma::mat33 &	R,
		const arma::vec3 &	p
	)

Converts a rotation matrix and a position vector into homogeneous transformation matrix.

Parameters

R	A 3x3 rotation matrix
p	A 3-vector

Returns

A homogeneous transformation matrix corresponding to the inputs

ScrewToAxis()

const arma::vec6 mr::ScrewToAxis	(	const arma::vec3 &	q,
		const arma::vec3 &	s,
		const double	h
	)

Takes a parametric description of a screw axis and converts it to a normalized screw axis.

Parameters

q	A point lying on the screw axis
s	A unit vector in the direction of the screw axis
h	The pitch of the screw axis

Returns

A normalized screw axis described by the inputs

se3ToVec()

const arma::vec6 mr::se3ToVec

(

const arma::mat44 &

se3mat

)

Converts an se3 matrix into a spatial velocity vector.

Parameters

se3mat

A 4x4 matrix in se3

Returns

The spatial velocity 6-vector corresponding to se3mat

so3ToVec()

const arma::vec3 mr::so3ToVec

(

const arma::mat33 &

so3mat

)

Converts an so(3) representation to a 3-vector.

Parameters

so3mat

A 3x3 skew-symmetric matrix

Returns

The 3-vector corresponding to so3mat

TestIfSE3()

bool mr::TestIfSE3 ( const arma::mat44 &  mat )

inline

Returns true if mat is close to or on the manifold SE(3)

Parameters

mat	A 4x4 matrix

Returns

True if mat is very close to or in SE(3), false otherwise

Computes the distance d from mat to the SE(3) manifold using the following method: Compute the determinant of the top 3x3 submatrix of mat. If det(mat) <= 0, d = a large number. If det(mat) > 0, replace the top 3x3 submatrix of mat with mat^T.mat, and set the first three entries of the fourth column of mat to zero. Then d = norm(T - I). If d is close to zero, return true. Otherwise, return false.

TestIfSO3()

bool mr::TestIfSO3 ( const arma::mat33 &  mat )

inline

Returns true if mat is close to or on the manifold SO(3)

Parameters

mat	A 3x3 matrix

Returns

True if mat is very close to or in SO(3), false otherwise

Computes the distance d from mat to the SO(3) manifold using the following method: If det(mat) <= 0, d = a large number. If det(mat) > 0, d = norm(mat^T.mat - I). If d is close to zero, return true. Otherwise, return false.

TransInv()

const arma::mat44 mr::TransInv

(

const arma::mat44 &

T

)

Inverts a homogeneous transformation matrix.

Parameters

T	A homogeneous transformation matrix

Returns

The inverse of T

Uses the structure of transformation matrices to avoid taking a matrix inverse, for efficiency.

TransToRp()

const std::tuple< const arma::mat33, const arma::vec3 > mr::TransToRp

(

const arma::mat44 &

T

)

Converts a homogeneous transformation matrix into a rotation matrix and position vector.

Parameters

T	A homogeneous transformation matrix

Returns

R: The corresponding rotation matrix,

p: The corresponding position vector.

VecTose3()

const arma::mat44 mr::VecTose3

(

const arma::vec6 &

V

)

Converts a spatial velocity vector into a 4x4 matrix in se3.

Parameters

V	A 6-vector representing a spatial velocity

Returns

The 4x4 se3 representation of V

VecToso3()

const arma::mat33 mr::VecToso3

(

const arma::vec3 &

omg

)

Converts a 3-vector to an so(3) representation.

Parameters

omg	A 3-vector

Returns

The skew symmetric representation of omg