Rigid Body Transformations#

A state object holds the variables of a body: its position, its velocity, and, for a rigid body, its attitude. Before these variables can change in time, they need a geometric meaning: in which frame a vector is expressed, and how the orientation of one frame relative to another is described.

This page covers that geometry, with no time involved: the frames of reference, the rotation matrices that move vectors between them, and the three representations of an attitude (Euler angles, rotation matrix, quaternion). The next page, Kinematics, lets these quantities change in time.

In c4dynamics, the geometry is implemented by the rotmat module, and by the BR and RB properties of the rigid body state objects.

Reference Frames#

A frame of reference (or a coordinate system) is an origin and three mutually perpendicular unit axes, \(x, y, z\). A physical vector, such as a velocity or a force, exists independently of any frame. Its three components, however, depend on the frame in which it is resolved. A vector resolved in a given frame is said to be expressed in that frame.

For example, an aircraft that flies north at 150 m/s and pitches up by \(30°\) has a single velocity vector. In an earth frame (north, east, down) its components are \([150, 0, 0]\), while in the aircraft body frame they are \([129.9, 0, 75]\) (see the examples below).

The rate of change of a vector, as viewed by an observer fixed to and moving with a given frame, is said to be relative to (or with respect to) that frame. The laws of motion hold relative to an inertial frame, but the vectors in them can be expressed in any frame.

The frames of c4dynamics

Symbol

Frame

Description

\(R\)

Reference

The frame in which the state objects express the position and the velocity (\(x, y, z, v_x, v_y, v_z\)), and against which the attitude of a body is defined.

\(I\)

Inertial

A frame that does not accelerate or rotate. The laws of motion hold relative to it.

\(B\)

Body

A frame fixed to the body and rotating with it. The origin is at the center of mass and \(x_b\) is along the centerline (the nose). The body angular rates \((p, q, r)\) and the body components of the velocity \((u, v, w)\) are expressed in this frame.

\(S\)

Sensor

A frame fixed to a sensor, such as a seeker or a radar. Its \(x\) axis is the sensor boresight.

Reference, inertial, and earth frames

The reference frame is a modeling choice, and is not necessarily inertial. The equations of motion of c4dynamics, however, are written relative to the reference frame (see Dynamics), and therefore assume that it is inertial. In the use cases, the reference frame is an earth frame: a frame fixed to a flat earth, such as NED or ENU below. The earth rotation and curvature are negligible over the time and the range of these use cases, so the earth frame is treated as inertial, and \(R\) and \(I\) denote the same frame. \(I\) is used where the inertial property matters, e.g. in the navigation sensors and the quadcopter model, and \(R\) elsewhere.

NED and ENU

Two pairs of an earth frame and a body frame are common for vehicles that move above a flat earth. Both are supported by the quadcopter model and the navigation sensors through the frame argument (see Quadcopter):

frame

Earth frame

Body frame

'NED'

\(x\) north, \(y\) east, \(z\) down

\(x\) forward, \(y\) right, \(z\) down (FRD)

'ENU'

\(x\) east, \(y\) north, \(z\) up

\(x\) forward, \(y\) left, \(z\) up (FLU)

Right-handed frames

All the frames in c4dynamics are right-handed:

\[\begin{split}\begin{aligned} i \times j &= k \\ j \times k &= i \\ k \times i &= j \end{aligned}\end{split}\]

where \(i, j, k\) are the unit vectors along the \(x, y, z\) axes. Positive rotations are clockwise when viewed from the origin, looking out along the positive direction of the axis (Fig-1).

../_images/frame_conventions.svg

Fig-1: Coordinate system conventions#

Rotation Matrices#

Vectors that are expressed in different frames are related through a Direction Cosine Matrix (DCM), a \(3 \times 3\) rotation matrix [HS].

Notation

c4dynamics names a rotation matrix by two letters in square brackets. The letters state the direction of the transformation, read from right to left: \([BR]\) (Body from Reference) takes a vector that is expressed in \(R\) and returns the same vector expressed in \(B\):

\[v_B = [BR] \cdot v_R\]

The same notation applies to any pair of frames, for example:

\([BR]\)

Body from Reference. Provided by rigidbody.BR and quatbody.BR

\([RB]\)

Reference from Body. Provided by rigidbody.RB and quatbody.RB

\([BI]\)

Body from Inertial, i.e. \([BR]\) when the reference frame is inertial. Used in the IMU, the magnetometer, and the quadcopter model

\([SB]\)

Sensor from Body, e.g. the gimbal of a seeker

Properties

Direction cosines. The element \([BR]_{ij}\) is the cosine of the angle between the \(i\)-th axis of \(B\) and the \(j\)-th axis of \(R\). Hence the rows of \([BR]\) are the body axes expressed in \(R\), and its columns are the reference axes expressed in \(B\).

Inverse. A rotation matrix is orthonormal, \([BR] \cdot [BR]^T = I_3\), and its determinant is \(+1\). Its inverse is its transpose, which is the transformation in the opposite direction:

\[\begin{split}\begin{aligned} [RB] &= [BR]^{-1} = [BR]^T \\ v_R &= [RB] \cdot v_B \end{aligned}\end{split}\]

Composition. A chain of transformations is composed by cancelling adjacent letters. For three frames \(A, B, C\):

\[[CA] = [CB] \cdot [BA]\]

For example, a seeker that is mounted on a gimbal sees the reference frame through \([SR] = [SB] \cdot [BR]\).

Passive rotation. The rotation matrices in c4dynamics are passive: they rotate the frame of coordinates rather than the vector. The vector itself is unchanged; only the frame in which its components are expressed changes. To rotate a vector actively by a positive angle within the same frame, use the transpose. E.g. \(R_z(\psi)^T \cdot v\) rotates \(v\) by \(\psi\) about \(z\).

Elementary rotations

A rotation of a frame through an angle about one of its axes is given by an elementary rotation matrix:

\[\begin{split}R_x(\varphi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\varphi) & \sin(\varphi) \\ 0 & -\sin(\varphi) & \cos(\varphi) \end{bmatrix} , \quad R_y(\theta) = \begin{bmatrix} \cos(\theta) & 0 & -\sin(\theta) \\ 0 & 1 & 0 \\ \sin(\theta) & 0 & \cos(\theta) \end{bmatrix} , \quad R_z(\psi) = \begin{bmatrix} \cos(\psi) & \sin(\psi) & 0 \\ -\sin(\psi) & \cos(\psi) & 0 \\ 0 & 0 & 1 \end{bmatrix}\end{split}\]

These are implemented by rotx, roty, and rotz. Any attitude is reached by a sequence of three elementary rotations.

Attitude Representations#

The attitude of a body is the orientation of its body frame \(B\) relative to the reference frame \(R\). It has three degrees of freedom, and c4dynamics represents it in three equivalent ways: three Euler angles, a rotation matrix \([BR]\), and a unit quaternion. The rotation matrix is the one that moves vectors between the frames. The Euler angles and the quaternion are the compact forms that are stored in the state vector and propagated in time (see Kinematics).

Euler Angles#

When Euler angles are employed in the transformation of a vector expressed in one reference frame to the expression of the vector in a different reference frame, any order of the three Euler rotations is possible, but the resulting transformation equations depend on the order selected [MIs].

In aerospace applications, the common order is that the first Euler rotation is about the z-axis, the second is about the y-axis, and the third is about the x-axis. Such a transformation order is called z-y-x, or 3-2-1. With reference to a body orientation, the resulting order is yaw, pitch, and roll. With reference to geographical orientation, the resulting order is azimuth (heading), elevation (pitch), and roll (bank angle).

c4dynamics adopts the 3-2-1 order. Starting with the body frame aligned with the reference frame:

  1. Rotate about \(z_b\) through the yaw (heading) angle \(\psi\),

  2. Rotate about \(y_b\) through the pitch angle \(\theta\),

  3. Rotate about \(x_b\) through the roll angle \(\varphi\).

The rotations are applied in the order yaw, pitch, roll. The angles, however, are listed in the opposite order, roll, pitch, yaw, \((\varphi, \theta, \psi)\), wherever they appear as a group: in the state vector of a rigidbody, in the arguments of dcm321 and euler2quat, and in the outputs of dcm321euler and quat2euler. This documentation follows the same order.

The first rotation is the rightmost in the product:

\[\begin{split}[BR] = R_x(\varphi) \cdot R_y(\theta) \cdot R_z(\psi) = \begin{bmatrix} c\theta \cdot c\psi & c\theta \cdot s\psi & -s\theta \\ s\varphi \cdot s\theta \cdot c\psi - c\varphi \cdot s\psi & s\varphi \cdot s\theta \cdot s\psi + c\varphi \cdot c\psi & s\varphi \cdot c\theta \\ c\varphi \cdot s\theta \cdot c\psi + s\varphi \cdot s\psi & c\varphi \cdot s\theta \cdot s\psi - s\varphi \cdot c\psi & c\varphi \cdot c\theta \end{bmatrix}\end{split}\]

where \(c \equiv \cos\) and \(s \equiv \sin\). This matrix is computed by dcm321.

The Euler angles are extracted back from the matrix by:

\[\begin{split}\begin{aligned} \varphi &= \operatorname{atan2}([BR]_{23}, [BR]_{33}) \\ \theta &= -\arcsin([BR]_{13}) \\ \psi &= \operatorname{atan2}([BR]_{12}, [BR]_{11}) \end{aligned}\end{split}\]

Gimbal lock. At \(\theta = \pm 90°\), \([BR]_{11}, [BR]_{12}, [BR]_{23}, [BR]_{33}\) all vanish, the roll axis is aligned with the yaw axis, and \(\varphi\) and \(\psi\) describe the same rotation. Only their sum (or difference) is defined, not each one alone. Any set of three angles has such a singular attitude. The consequence for the propagation of the angles in time is discussed in Kinematics.

Quaternion#

According to Euler’s rotation theorem, any attitude is reached from the reference frame by a single rotation through an angle \(\alpha\) about a unit axis \(n = [n_x, n_y, n_z]\). The attitude quaternion encodes this rotation by four parameters, scalar first (the Hamilton convention):

\[q = [q_w, q_x, q_y, q_z] = [\cos{\alpha \over 2}, \; n_x \cdot \sin{\alpha \over 2}, \; n_y \cdot \sin{\alpha \over 2}, \; n_z \cdot \sin{\alpha \over 2}]\]

Where:

  • \(q_w\) is the scalar part of the quaternion

  • \(q_x, q_y, q_z\) are the vector part of the quaternion

Four parameters describe three degrees of freedom, therefore the quaternion is subject to one constraint. An attitude quaternion is a unit quaternion:

\[q_w^2 + q_x^2 + q_y^2 + q_z^2 = 1\]

\(q\) and \(-q\) represent the same attitude (a rotation of \(\alpha\) about \(n\) equals a rotation of \(\alpha - 360°\) about the same axis). Unlike the three Euler angles, the quaternion has no singular attitude [DI].

Rotation matrix

The quaternion describes the rotation that carries the reference frame onto the body frame. The matrix that c4dynamics computes from a quaternion is the Body from Reference DCM, \([BR]\): it takes a vector expressed in the reference frame and returns the same vector expressed in the body frame, \(v_B = [BR] \cdot v_R\):

\[\begin{split}[BR] = \begin{bmatrix} 1 - 2(q_y^2 + q_z^2) & 2(q_x q_y + q_w q_z) & 2(q_x q_z - q_w q_y) \\ 2(q_x q_y - q_w q_z) & 1 - 2(q_x^2 + q_z^2) & 2(q_y q_z + q_w q_x) \\ 2(q_x q_z + q_w q_y) & 2(q_y q_z - q_w q_x) & 1 - 2(q_x^2 + q_y^2) \end{bmatrix}\end{split}\]

Some references give instead the matrix that rotates a vector by \(\alpha\) about \(n\). That matrix is the transpose, \([RB]\), and its off-diagonal terms that contain \(q_w\) have opposite signs. The difference is in the direction of the transformation, not a sign error. In quaternion products, \(v_B = [BR] \cdot v_R\) is \([0, v_B] = q^* \otimes [0, v_R] \otimes q\), where \(q^* = [q_w, -q_x, -q_y, -q_z]\) is the conjugate of \(q\).

For example, the quaternion of a yaw of \(90°\), \(q = [\cos 45°, 0, 0, \sin 45°]\), gives \([BR] = R_z(90°)\), the elementary rotation above. In general, a quaternion describes the same attitude as the 3-2-1 Euler angles of the convention above, and its \([BR]\) is identical to dcm321 of those angles. It is computed by quat2dcm.

Relation to the Euler angles

The quaternion of the attitude that is defined by the 3-2-1 Euler angles \((\varphi, \theta, \psi)\) is given by:

\[\begin{split}\begin{aligned} q_w &= c{\varphi \over 2} c{\theta \over 2} c{\psi \over 2} + s{\varphi \over 2} s{\theta \over 2} s{\psi \over 2} \\ q_x &= s{\varphi \over 2} c{\theta \over 2} c{\psi \over 2} - c{\varphi \over 2} s{\theta \over 2} s{\psi \over 2} \\ q_y &= c{\varphi \over 2} s{\theta \over 2} c{\psi \over 2} + s{\varphi \over 2} c{\theta \over 2} s{\psi \over 2} \\ q_z &= c{\varphi \over 2} c{\theta \over 2} s{\psi \over 2} - s{\varphi \over 2} s{\theta \over 2} c{\psi \over 2} \end{aligned}\end{split}\]

The inverse relation, from a quaternion to the Euler angles, is given by:

\[\begin{split}\begin{aligned} \varphi &= \operatorname{atan2}(2(q_w q_x + q_y q_z), \; 1 - 2(q_x^2 + q_y^2)) \\ \theta &= \arcsin(2(q_w q_y - q_x q_z)) \\ \psi &= \operatorname{atan2}(2(q_w q_z + q_x q_y), \; 1 - 2(q_y^2 + q_z^2)) \end{aligned}\end{split}\]

The extracted Euler angles are, of course, still singular at \(\theta = \pm 90°\). The quaternion itself is not. The quatbody state object therefore stores the quaternion, and computes the Euler angles only for presentation.

Conversions#

The functions of the rotmat module convert between the three representations:

From \ To

Euler angles \((\varphi, \theta, \psi)\)

DCM \([BR]\)

Quaternion \(q\)

Euler angles

-

dcm321

euler2quat

DCM

dcm321euler (degrees)

-

euler2quat(*dcm321euler(BR) * d2r)

Quaternion

quat2euler

quat2dcm

-

Notes:

  • All the functions take and return angles in radians, except dcm321euler, which returns degrees.

  • All the matrices are \([BR]\). To get \([RB]\), transpose the result.

  • A conversion that passes through the Euler angles inherits their singularity at \(\theta = \pm 90°\).

Between ENU and NED

The ENU and the NED frames swap \(x\) and \(y\) and flip \(z\). The matrix that does it is its own inverse, so the same matrix converts both ways:

\[\begin{split}v_{NED} = \begin{bmatrix} 0 & 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & -1 \end{bmatrix} \cdot v_{ENU}\end{split}\]

A heading measured from east (ENU) becomes a heading measured from north (NED): \(\psi_{NED} = \pi / 2 - \psi_{ENU}\). This conversion is used at the MAVLink boundary of the ArduPilot simulation.

Examples#

Import c4dynamics and numpy:

>>> import c4dynamics as c4d
>>> import numpy as np

Elementary rotations

A frame that is rotated by \(90°\) about \(z\). The \(x\) axis of the original frame is the \(-y\) axis of the rotated frame:

>>> c4d.rotmat.rotz(90 * c4d.d2r) @ [1, 0, 0]  
[0  -1  0]

The transpose rotates the vector itself (active rotation) by \(90°\) about \(z\), from \(x\) to \(y\):

>>> c4d.rotmat.rotz(90 * c4d.d2r).T @ [1, 0, 0]  
[0  1  0]

The 3-2-1 matrix is the product of the three elementary rotations:

>>> phi, theta, psi = 10 * c4d.d2r, 20 * c4d.d2r, 30 * c4d.d2r
>>> BR = c4d.rotmat.dcm321(phi, theta, psi)
>>> np.allclose(BR, c4d.rotmat.rotx(phi) @ c4d.rotmat.roty(theta) @ c4d.rotmat.rotz(psi))
True

It is orthonormal, so its transpose is its inverse:

>>> np.allclose(BR @ BR.T, np.eye(3))
True
>>> np.linalg.det(BR)  
1

From reference to body and back

The inertial velocity of an aircraft flying north, expressed in an earth frame, and its attitude, a pitch of \(30°\):

>>> v_R = [150, 0, 0]
>>> BR = c4d.rotmat.dcm321(theta = 30 * c4d.d2r)

The velocity expressed in the body frame, \(v_B = [BR] \cdot v_R\):

>>> v_B = BR @ v_R
>>> v_B  
[129.9  0  75]

And back to the earth frame, \(v_R = [RB] \cdot v_B = [BR]^T \cdot v_B\):

>>> BR.T @ v_B  
[150  0  0]

The body axes in the reference frame

A body with heading \(\psi = 90°\) and pitch \(\theta = 30°\). \([BR]\) expresses the reference \(x\) axis in body coordinates, and \([RB] = [BR]^T\) expresses the body \(x\) axis (the nose) in reference coordinates. The latter is also the first row of \([BR]\):

>>> BR = c4d.rotmat.dcm321(theta = 30 * c4d.d2r, psi = 90 * c4d.d2r)
>>> BR @ [1, 0, 0]  
[0  -1  0]
>>> BR.T @ [1, 0, 0]  
[0  0.866  -0.5]
>>> BR[0]  
[0  0.866  -0.5]

The state objects provide these matrices for their current attitude:

>>> rb = c4d.rigidbody(theta = 30 * c4d.d2r, psi = 90 * c4d.d2r)
>>> rb.RB @ [1, 0, 0]  
[0  0.866  -0.5]

Composing transformations

A target position relative to a seeker, expressed in the reference frame, and a seeker body with a heading of \(30°\):

>>> dx = [1000, 1000, 0]
>>> skr = c4d.rigidbody(psi = 30 * c4d.d2r)

The relative position in the body frame and the azimuth angle to the target:

>>> x_b = skr.BR @ dx
>>> x_b  
[1366  366  0]
>>> np.arctan2(x_b[1], x_b[0]) * c4d.r2d  
15

A gimbal that turns the sensor by another \(15°\) about \(z\) gives \([SB]\). The sensor sees the reference frame through \([SR] = [SB] \cdot [BR]\), which puts the target on its boresight:

>>> SB = c4d.rotmat.rotz(15 * c4d.d2r)
>>> SR = SB @ skr.BR
>>> SR @ dx  
[1414.2  0  0]

The two \(z\) rotations add up:

>>> np.allclose(SR, c4d.rotmat.rotz(45 * c4d.d2r))
True

Converting between representations

From the Euler angles to the DCM and back. Note that dcm321euler returns degrees:

>>> BR = c4d.rotmat.dcm321(10 * c4d.d2r, 20 * c4d.d2r, 30 * c4d.d2r)
>>> c4d.rotmat.dcm321euler(BR)  
(10  20  30)

From the Euler angles to a quaternion, and from the quaternion to the DCM and to the Euler angles (in radians):

>>> q = c4d.rotmat.euler2quat(10 * c4d.d2r, 20 * c4d.d2r, 30 * c4d.d2r)
>>> q  
[0.952  0.038  0.189  0.239]
>>> np.allclose(c4d.rotmat.quat2dcm(q), BR)
True
>>> c4d.rotmat.quat2euler(q) * c4d.r2d  
[10  20  30]

From the DCM to a quaternion, through the Euler angles:

>>> np.allclose(c4d.rotmat.euler2quat(*np.array(c4d.rotmat.dcm321euler(BR)) * c4d.d2r), q)
True

The matrix of a quaternion is the Body from Reference DCM. A yaw of \(90°\) gives the elementary rotation \(R_z(90°)\):

>>> q90 = [np.cos(45 * c4d.d2r), 0, 0, np.sin(45 * c4d.d2r)]
>>> np.allclose(c4d.rotmat.quat2dcm(q90), c4d.rotmat.rotz(90 * c4d.d2r))
True

\(q\) and \(-q\) are the same attitude:

>>> np.allclose(c4d.rotmat.quat2dcm(-q), c4d.rotmat.quat2dcm(q))
True

quat2euler also converts an array of quaternions, e.g. the attitude history of a quatbody:

>>> c4d.rotmat.quat2euler(np.array([q, -q])) * c4d.r2d  
[[10  20  30]
 [10  20  30]]

Between ENU and NED

>>> enu2ned = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
>>> enu2ned @ [3, 4, 5]  
[4  3  -5]
>>> np.allclose(enu2ned @ enu2ned, np.eye(3))
True

References#

[HS]

Hanspeter Schaub, “Spacecraft Dynamics and Control” lecture notes, module 2: rigidbody kinematics.

[MIs]

Ch 4 in “Missile Flight Simulation Part One Surface-to-Air Missiles”, Military Handbook, 1995, MIL-HDBK-1211(MI).

[DI]

Diebel, J., 2006, “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors”, Stanford University.

See Also#

Kinematics

Next: the propagation of the position and the attitude in time

rotx, roty, rotz

Direction Cosine Matrices for a rotation about the \(x\), \(y\), \(z\) axes

dcm321

Direction Cosine Matrix for a sequence of rotations in following order: \(z\), then \(y\), then \(x\)

dcm321euler

Extract Euler angles (roll, pitch, yaw) from a Direction Cosine Matrix (DCM) of 3-2-1 order

euler2quat

Attitude quaternion of 3-2-1 Euler angles

quat2euler

3-2-1 Euler angles of an attitude quaternion (radians)

quat2dcm

Body from Reference Direction Cosine Matrix of an attitude quaternion

rigidbody

Rigid body object with an Euler angles attitude

quatbody

Rigid body object with a quaternion attitude

animate

Animates the rigid body’s motion using a 3D model according to the 3-2-1 Euler angles histories.