M-File Help: UnitQuaternion View code for UnitQuaternion

UnitQuaternion

Unit quaternion class

A UnitQuaternion 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 vector v and is typically written: q = s <vx, vy, vz>.

A UnitQuaternion is one for which sˆ2+vxˆ2+vyˆ2+vzˆ2 = 1. It can be considered as a rotation by an angle theta about a unit-vector V in space where

q = cos (theta/2) < v sin(theta/2)>

Constructors

UnitQuaternion general constructor
UnitQuaternion.eul constructor, from Euler angles
UnitQuaternion.rpy constructor, from roll-pitch-yaw angles
UnitQuaternion.angvec constructor, from (angle and vector)
UnitQuaternion.omega constructor for angle*vector
UnitQuaternion.Rx constructor, from x-axis rotation
UnitQuaternion.Ry constructor, from y-axis rotation
UnitQuaternion.Rz constructor, from z-axis rotation
UnitQuaternion.vec constructor, from 3-vector

Display methods

display print in human readable form
plot plot a coordinate frame representing orientation of quaternion
animate animates a coordinate frame representing changing orientation of quaternion sequence

Operation methods

inv inverse
conj conjugate
unit unitized quaternion
dot derivative of quaternion with angular velocity
norm norm, or length
inner inner product
angle angle between two quaternions
interp interpolation (slerp) between two quaternions
UnitQuaternion.qvmul multiply unit-quaternions in 3-vector form

Conversion methods

char convert to string
double convert to 4-vector
matrix convert to 4x4 matrix
tovec convert to 3-vector
R convert to 3x3 rotation matrix
T convert to 4x4 homogeneous transform matrix
toeul convert to Euler angles
torpy convert to roll-pitch-yaw angles
toangvec convert to angle vector form
SO3 convert to SO3 class
SE3 convert to SE3 class

Overloaded operators

q*q2 quaternion (Hamilton) product
q.*q2 quaternion (Hamilton) product followed by unitization
q*s quaternion times scalar
q/q2 q*q2.inv
q./q2 q*q2.inv followed by unitization
q/s quaternion divided by scalar
q^n q to power n (integer only)
q+q2 elementwise sum of quaternion elements (result is a Quaternion)
q-q2 elementwise difference of quaternion elements (result is a Quaternion)
q1==q2 test for quaternion equality
q1~=q2 test for quaternion inequality

Properties (read only)

s real part
v vector part

Notes

since the result is not, in general, a valid UnitQuaternion.

References

See also

Quaternion, SO3


UnitQuaternion.UnitQuaternion

Create a unit quaternion object

Construct a UnitQuaternion from various other orientation representations.

q = UnitQuaternion() is the identitity UnitQuaternion 1&lt;0,0,0&gt; representing a null rotation.

q = UnitQuaternion(q1) is a copy of the UnitQuaternion q1, if q1 is a Quaternion it is normalised.

q = UnitQuaternion(s, v) is a unit quaternion formed by specifying directly its scalar and vector parts which are normalised.

q = UnitQuaternion([s V1 V2 V3]) is a quaternion formed by specifying directly its 4 elements which are normalised.

q = Quaternion(R) is a UnitQuaternion corresponding to the SO(3) orthonormal rotation matrix R (3x3). If R (3x3xN) is a sequence then q (Nx1) is a vector of Quaternions corresponding to the elements of R.

q = Quaternion(T) is a UnitQuaternion equivalent to the rotational part of the SE(3) homogeneous transform T (4x4). If T (4x4xN) is a sequence then q (Nx1) is a vector of Quaternions corresponding to the elements of T.

Notes

See also UnitQuaternion.eul, UnitQuaternion.rpy, UnitQuaternion.angvec, UnitQuaternion.omega, UnitQuaternion.Rx, UnitQuaternion.Ry, UnitQuaternion.Rz.


UnitQuaternion.angle

Angle between two UnitQuaternions

Q1.theta(q2) is the angle (in radians) between two UnitQuaternions Q1 and q2.

Notes

References

See also

Quaternion.angvec


UnitQuaternion.angvec

Construct from angle and rotation vector

q = UnitQuaternion.angvec(th, v) is a UnitQuaternion representing rotation of th about the vector v (3x1).

See also

UnitQuaternion.omega


UnitQuaternion.animate

Animate a quaternion object

Q.animate(options) animates a quaternion array Q as a 3D coordinate frame.

Q.animate(qf, options) animates a 3D coordinate frame moving from orientation Q to orientation qf.

Options

Options are passed to tranimate and include:

'fps', fps Number of frames per second to display (default 10)
'nsteps', n The number of steps along the path (default 50)
'axis', A Axis bounds [xmin, xmax, ymin, ymax, zmin, zmax]
'movie', M Save frames as files in the folder M
'cleanup' Remove the frame at end of animation
'noxyz' Don't label the axes
'rgb' Color the axes in the order x=red, y=green, z=blue
'retain' Retain frames, don't animate

Additional options are passed through to TRPLOT.

See also

tranimate, trplot


UnitQuaternion.char

Convert to string

s = Q.char() is a compact string representation of the quaternion's value as a 4-tuple. If Q is a vector then s has one line per element.

See also

Quaternion.char


UnitQuaternion.dot

Quaternion derivative

qd = Q.dot(omega) is the rate of change in the world frame of a body frame with attitude Q and angular velocity OMEGA (1x3) expressed as a quaternion.

Notes

Reference

See also

UnitQuaternion.dotb


UnitQuaternion.dotb

Quaternion derivative

qd = Q.dot(omega) is the rate of change in the body frame of a body frame with attitude Q and angular velocity OMEGA (1x3) expressed as a quaternion.

Notes

Reference

See also

UnitQuaternion.dot


UnitQuaternion.eul

Construct from Euler angles

q = UnitQuaternion.eul(phi, theta, psi, options) is a UnitQuaternion representing rotation equivalent to the specified Euler angles angles. These correspond to rotations about the Z, Y, Z axes respectively.

q = UnitQuaternion.eul(eul, options) as above but the Euler angles are taken from the vector (1x3) eul = [phi theta psi]. If eul is a matrix (Nx3) then q is a vector (1xN) of UnitQuaternion objects where the index corresponds to rows of eul which are assumed to be [phi,theta,psi].

Options

'deg' Compute angles in degrees (radians default)

Notes

See also

UnitQuaternion.rpy, eul2r


UnitQuaternion.increment

Update quaternion by angular displacement

qu = Q.increment(omega) updates Q by a rotation which is given as a spatial displacement omega (3x1) whose direction is the rotation axis and magnitude is the amount of rotation.

See also

tr2delta


UnitQuaternion.interp

Interpolate UnitQuaternions

qi = Q.scale(s, options) is a UnitQuaternion that interpolates between a null rotation (identity quaternion) for s=0 to Q for s=1.

qi = Q.interp(q2, s, options) as above but interpolates a rotation between Q for s=0 and q2 for s=1.

If s is a vector qi is a vector of UnitQuaternions, each element corresponding to sequential elements of s.

Options

'shortest' Take the shortest path along the great circle

Notes

References

See also

ctraj


UnitQuaternion.inv

Invert a UnitQuaternion

qi = Q.inv() is a UnitQuaternion object representing the inverse of Q.

Notes


UnitQuaternion.mrdivide

Divide unit quaternions

Q1/Q2 is a UnitQuaternion object formed by Hamilton product of Q1 and

inv(q2) where Q1 and q2 are both UnitQuaternion objects.

Notes

See also

Quaternion.mtimes, Quaternion.mpower, Quaternion.plus, Quaternion.minus


UnitQuaternion.mtimes

Multiply unit quaternions

Q1*Q2 is a UnitQuaternion object formed by Hamilton product

of Q1 and Q2 where Q1 and Q2 are both UnitQuaternion objects.

Q*V is a vector (3x1) formed by rotating the vector V (3x1)by the UnitQuaternion Q.

Notes

See also

Quaternion.mrdivide, Quaternion.mpower, Quaternion.plus, Quaternion.minus


UnitQuaternion.new

Construct a new unit quaternion

qn = Q.new() constructs a new UnitQuaternion object of the same type as Q.

qn = Q.new([S V1 V2 V3]) as above but specified directly by its 4 elements.

qn = Q.new(s, v) as above but specified directly by the scalar s and vector part v (1x3)

Notes


UnitQuaternion.omega

Construct from angle times rotation vector

q = UnitQuaternion.omega(w) is a UnitQuaternion representing rotation of |w| about the vector w (3x1).

See also

UnitQuaternion.angvec


UnitQuaternion.plot

Plot a quaternion object

Q.plot(options) plots the quaternion as an oriented coordinate frame.

H = Q.plot(options) as above but returns a handle which can be used for animation.

Animation

Firstly, create a plot and keep the the handle as per above.

Q.plot('handle', H) updates the coordinate frame described by the handle H to the orientation of Q.

Options

Options are passed to trplot and include:

'color', C The color to draw the axes, MATLAB colorspec C
'frame', F The frame is named {F} and the subscript on the axis labels is F.
'view', V Set plot view parameters V=[az el] angles, or 'auto' for view toward origin of coordinate frame
'handle', h Update the specified handle

See also

trplot


UnitQuaternion.q2r

Convert UnitQuaternion to homogeneous transform

T = q2tr(q)

Return the rotational homogeneous transform corresponding to the unit quaternion q.

See also: TR2Q


UnitQuaternion.qvmul

Multiply unit quaternions defined by vector part

qv = UnitQuaternion.QVMUL(qv1, qv2) multiplies two unit-quaternions defined only by their vector components qv1 and qv2 (3x1). The result is similarly the vector component of the product (3x1).

See also

UnitQuaternion.tovec, UnitQuaternion.vec


UnitQuaternion.R

Convert to orthonormal rotation matrix

R = Q.R() is the equivalent SO(3) orthonormal rotation matrix (3x3). If Q represents a sequence (Nx1) then R is 3x3xN.

See also

UnitQuaternion.T, UnitQuaternion.SO3


UnitQuaternion.rdivide

Divide unit quaternions and unitize

Q1./Q2 is a UnitQuaternion object formed by Hamilton product of Q1 and

inv(q2) where Q1 and q2 are both UnitQuaternion objects. The result is explicitly unitized.

Notes

See also

Quaternion.mtimes


UnitQuaternion.rpy

Construct from roll-pitch-yaw angles

q = UnitQuaternion.rpy(roll, pitch, yaw, options) is a UnitQuaternion representing rotation equivalent to the specified roll, pitch, yaw angles angles. These correspond to rotations about the Z, Y, X axes respectively.

q = UnitQuaternion.rpy(rpy, options) as above but the angles are given by the passed vector rpy = [roll, pitch, yaw]. If rpy is a matrix (Nx3) then q is a vector (1xN) of UnitQuaternion objects where the index corresponds to rows of rpy which are assumed to be [roll,pitch,yaw].

Options

'deg' Compute angles in degrees (radians default)
'xyz' Return solution for sequential rotations about X, Y, Z axes.
'yxz' Return solution for sequential rotations about Y, X, Z axes.

UnitQuaternion.Rx

Construct from rotation about x-axis

q = UnitQuaternion.Rx(angle) is a UnitQuaternion representing rotation of angle about the x-axis.

q = UnitQuaternion.Rx(angle, 'deg') as above but THETA is in degrees.

See also

UnitQuaternion.Ry, UnitQuaternion.Rz


UnitQuaternion.Ry

Construct from rotation about y-axis

q = UnitQuaternion.Ry(angle) is a UnitQuaternion representing rotation of angle about the y-axis.

q = UnitQuaternion.Ry(angle, 'deg') as above but THETA is in degrees.

See also

UnitQuaternion.Rx, UnitQuaternion.Rz


UnitQuaternion.Rz

Construct from rotation about z-axis

q = UnitQuaternion.Rz(angle) is a UnitQuaternion representing rotation of angle about the z-axis.

q = UnitQuaternion.Rz(angle, 'deg') as above but THETA is in degrees.

See also

UnitQuaternion.Rx, UnitQuaternion.Ry


UnitQuaternion.SE3

Convert to SE3 object

x = Q.SE3() is an SE3 object with equivalent rotation and zero translation.

Notes

See also

UnitQuaternion.SE3, SE3


UnitQuaternion.SO3

Convert to SO3 object

x = Q.SO3() is an SO3 object with equivalent rotation.

Notes

See also

UnitQuaternion.SE3, SO3


UnitQuaternion.T

Convert to homogeneous transformation matrix

T = Q.T() is the equivalent SE(3) homogeneous transformation matrix (4x4). If Q is a sequence (Nx1) then T is 4x4xN.

Notes:

See also

UnitQuaternion.R, UnitQuaternion.SE3


UnitQuaternion.times

Multiply a quaternion object and unitize

Q1.*Q2 is a UnitQuaternion object formed by Hamilton product of Q1 and

Q2. The result is explicitly unitized.

Notes

See also

Quaternion.mtimes


UnitQuaternion.toangvec

Convert to angle-vector form

th = Q.angvec(options) is the rotational angle, about some vector, corresponding to this quaternion.

[th,v] = Q.angvec(options) as above but also returns a unit vector parallel to the rotation axis.

Q.angvec(options) prints a compact single line representation of the rotational angle and rotation vector corresponding to this quaternion.

Options

'deg' Display/return angle in degrees rather than radians

Notes


UnitQuaternion.toeul

Convert to roll-pitch-yaw angle form.

eul = Q.toeul(options) are the Euler angles (1x3) corresponding to the UnitQuaternion. These correspond to rotations about the Z, Y, Z axes respectively. eul = [PHI,THETA,PSI].

Options

'deg' Compute angles in degrees (radians default)

Notes

See also

UnitQuaternion.toeul, tr2rpy


UnitQuaternion.torpy

Convert to roll-pitch-yaw angle form.

rpy = Q.torpy(options) are the roll-pitch-yaw angles (1x3) corresponding to the UnitQuaternion. These correspond to rotations about the Z, Y, X axes respectively. rpy = [ROLL, PITCH, YAW].

Options

'deg' Compute angles in degrees (radians default)
'xyz' Return solution for sequential rotations about X, Y, Z axes
'yxz' Return solution for sequential rotations about Y, X, Z axes

Notes

See also

UnitQuaternion.toeul, tr2rpy


UnitQuaternion.tovec

Convert to unique 3-vector

v = Q.tovec() is a vector (1x3) that uniquely represents the UnitQuaternion. The scalar component can be recovered by 1 - norm(v) and will always be positive.

Notes

See also

UnitQuaternion.vec, UnitQuaternion.qvmul


UnitQuaternion.tr2q

Convert homogeneous transform to a UnitQuaternion

q = tr2q(T)

Return a UnitQuaternion corresponding to the rotational part of the homogeneous transform T.


UnitQuaternion.vec

Construct from 3-vector

q = UnitQuaternion.vec(v) is a UnitQuaternion constructed from just its vector component (1x3) and the scalar part is 1 - norm(v) and will always be positive.

Notes

See also

UnitQuaternion.tovec, UnitVector.qvmul


 

© 1990-2014 Peter Corke.