Geometry module¶
The inertialsim.geometry module provides geometric data types and operations. Geometry types are built on the MatrixLieGroup abstract base class. Lie groups are mathematical objects that can concisely represent common geometric operations like translation, rotation, and rigid body motion. Lie groups allow for common implementations of standard operations like composition and interpolation. Users of InertialSim do not need to be aware of any of the mathematical formalisms but those interested can consult Reference [15].
Geometry type hierarchy¶
The geometry type hierarchy is illustrated below (note: the relationships are
simplified for clarity). Vector and
Rotation types are the basic building blocks
implementing linear and rotational motion.
RigidTranslation is a composition of
simultaneous linear and rotational motion.
Translation and
Pose are aliases for Vector and RigidTransform
respectively. ExtendedPose adds velocity
to poses.
---
config:
class:
hideEmptyMembersBox: true
---
classDiagram
direction BT
Vector --|> MatrixLieGroup
Translation --|> Vector
RigidTransform --|> MatrixLieGroup
Rotation --|> MatrixLieGroup
Translation --o RigidTransform
Pose --|> RigidTransform
Rotation --o RigidTransform
ExtendedPose --|> Pose
class MatrixLieGroup{
<<Abstract>>
time: np.ndarray
array: np.ndarray
interpolate()
compose()
apply()
\__getitem__()
\__matmul__()
}
class Vector{
from_cartesian()
from_spherical()
}
class Translation{
}
class Rotation{
from_matrix()
from_euler()
from_quaternion()
}
class RigidTransform{
R: Rotation
t: Translation
}
class ExtendedPose{
R: Rotation
t: Translation
v: Vector
}Geometry type patterns¶
All geometry types store their underlying data and an optional time attribute.
The time attribute must have unique, sorted values that index the geometry
data to form a time series.
All geometry types should be initialized using the factory methods, named
from_*(), e.g. from_matrix. All
common coordinate systems (Cartesian, spherical, etc.) and data formats (Euler
angles, quaternions, homogeneous matrix, etc.) are supported.
A common apply method is provided that applies the action of the type
(translation, rotation, etc.) to the inputs. Individual types also implement
the *, +, and @ operators which can be used similarly.
Active and passive conventions¶
All geometry operations in InertialSim use the active convention.
Active and passive conventions are often unstated or misunderstood, especially in the context of rotations. InertialSim conventions are clarified here. In the active convention, objects are translated, rotated, or transformed (simultaneous translation and rotation) with respect to a fixed coordinate system. An obvious example is an object that is moved by a robotic arm relative to a workspace coordinate system. In the passive convention, fixed objects are represented in different coordinate systems. An example is the gravity vector at a point on Earth. It can be measured in many coordinate systems, with two common examples being a local vertical coordinate system and a sensor coordinate system.
Specifically referring to rotational motion most references distinguish the two
conventions with the names rotation (active) and coordinate transform (passive)
(see Reference [5]). Because we already use Transform in the
context of RigidTransform we simply use
CoordinateTranslation,
CoordinateRotation, and
CoordinateTransform.
To illustrate, we demonstrate the effect of each class and convention on a random set of 5 points in 3D space. We will visualize the examples in 2D for simplicity.
# -----------------------------------------------------------------------------
# Copyright (c) 2023-2025, Inertial Simulation LLC.
# All rights reserved.
# Do not use or redistribute without permission.
# Email: info@inertialsim.com
# -----------------------------------------------------------------------------
%matplotlib widget
import numpy as np
from inertialsim import plot
# Each geometry class comes in a standard and coordinate version
from inertialsim.geometry import (
CoordinateRotation,
CoordinateTransform,
CoordinateTranslation,
RigidTransform,
Rotation,
Translation,
)
# Create 5 random 3D points and plot them
rng = np.random.default_rng(seed=4)
points = rng.random(size=(5, 3, 1))
pts = plot.Scatter(title="Random Points", xlabel="X", ylabel="Y")
pts.scatter(points[:, 0], points[:, 1], xlimits=[-3, 4], ylimits=[-3, 4], color="blue")
Translations¶
Next we will create Translation and CoordinateTranslation instances and
apply them to these points.
# Translate by 1 unit in X and 2 units in Y
x = 1
y = 2
z = 0
translation = Translation.from_cartesian([x, y, 0])
coordinate_translation = CoordinateTranslation.from_cartesian([x, y, 0])
points_translated = translation.apply(points)
coordinates_translated = coordinate_translation.apply(points)
tpts = plot.Scatter(title="Translated Points", xlabel="X", ylabel="Y")
tpts.scatter(points[:, 0], points[:, 1], xlimits=[-3, 4], ylimits=[-3, 4], color="blue")
tpts.scatter(points_translated[:, 0], points_translated[:, 1], color="red")
tpts.scatter(coordinates_translated[:, 0], coordinates_translated[:, 1], color="black")
tpts.legend(["Original", "Translated points", "Translated coordinates"])
tpts.axis_arrows()
tpts.axis_arrows(x, y, alpha=0.3)
As would be expected the effect of applying a Translation is to shift the
points. The effect of applying a CoordinateTranslation is to shift the origin
of the coordinate system (shown as semi-transparent axes) which affects the
coordinates of the points. The results are inverses of each other.
Rotations¶
Next we will create Rotation and CoordinateRotation instances and
apply them to the original points.
Note that the z-axis (not shown in plots) points up and out of the screen in order to form a right-handed triad with x and y axes. A positive rotation uses the right-hand rule (thumb pointing along axis, rotation in the direction of finger curl) and is therefore counter-clockwise.
# Rotate by 60 degrees (pi/3 radians) around Z
angle = np.pi / 3
rotation = Rotation.from_axis_angle([0, 0, 1], angle)
coordinate_rotation = CoordinateRotation.from_axis_angle([0, 0, 1], angle)
points_rotated = rotation.apply(points)
coordinates_rotated = coordinate_rotation.apply(points)
rpts = plot.Scatter(title="Rotated Points", xlabel="X", ylabel="Y")
rpts.scatter(points[:, 0], points[:, 1], xlimits=[-3, 4], ylimits=[-3, 4], color="blue")
rpts.scatter(points_rotated[:, 0], points_rotated[:, 1], color="red")
rpts.scatter(coordinates_rotated[:, 0], coordinates_rotated[:, 1], color="black")
rpts.legend(["Original points", "Rotated points", "Rotated coordinates"])
rpts.axis_arrows()
rpts.axis_arrows(angle=angle, alpha=0.3)
As with the translation examples, the results are inverses of each other.
Rigid Transforms¶
In the final example we create RigidTransform and CoordinateTransform
instances and apply them to the original points.
# Rotate and translate simultaneously
transform = RigidTransform.from_rotation_translation(rotation, translation)
coordinate_transform = CoordinateTransform.from_rotation_translation(
coordinate_rotation,
coordinate_translation,
)
points_transformed = transform.apply(points)
coordinates_transformed = coordinate_transform.apply(points)
tfpts = plot.Scatter(title="Transformed Points", xlabel="X", ylabel="Y")
tfpts.scatter(points[:, 0], points[:, 1], xlimits=[-3, 4], ylimits=[-3, 4], color="blue")
tfpts.scatter(points_transformed[:, 0], points_transformed[:, 1], color="red")
tfpts.scatter(
coordinates_transformed[:, 0], coordinates_transformed[:, 1], color="black"
)
tfpts.legend(["Original points", "Transformed points", "Transformed coordinates"])
tfpts.axis_arrows()
tfpts.axis_arrows(x=x, y=y, angle=angle, alpha=0.3)
The pattern remains and the two results are inverses of each other.
Pose conventions¶
Pose is an alias for RigidTransform but this is simply a mathematical
statement. The physical meaning of the rotation and translation components are
subject to different conventions in different scientific and engineering
disciplines. We define the InertialSim conventions and their relation to other
conventions here.
In the inertial navigation field it is conventional to define attitude as the coordinate rotation between body and navigation coordinate systems, see References 1, 2, 21, 22, and 27 for example. The body coordinate system is equivalent to the Input Reference Axes (IRA) marked on the case of the sensor as defined in Standards 03, 05, and 06. The navigation frame depends on the application and is one of: a local Cartesian frame; a topocentric (or local-level) frame fixed at a point on Earth's surface; the Earth-centered, Earth-fixed (ECEF) frame; or a fixed inertial frame. This attitude is typically denoted:
\(\mathbf{C}\) for coordinate rotation, \(\mathbf{b}\) for body and \(\mathbf{n}\) for navigation. Because rotations and coordinate rotations are inverses (see above), this is equivalent to the rotation from navigation to body frame:
Position is conventionally defined as the Cartesian, spherical, or ellipsoidal coordinates of the vector from navigation frame origin to body frame origin in navigation frame coordinates. This is typically denoted:
Considering the pose as a rigid transform (with the position in Cartesian components), when applied to points in body coordinates they are transformed to navigation coordinates.
Euler angle conventions¶
Inertial navigation and automotive Euler angle conventions define a sequence of
ZYX intrinsic coordinate rotations from navigation to body frame. See the
inertial navigation references above and automotive Standard [02].
This results in \(\mathbf{C_{n}^{b}}\) which is the transpose of
\(\mathbf{C_{b}^{n}}\) as defined above. InertialSim supports this convention
(and any other set of intrinsic coordinate rotations) via the
from_euler constructor of
the CoordinateRotation class.
InertialSim's preferred convention is to use the Rotation class which supports
any sequence of extrinsic rotations via the
from_euler constructor. As shown
above, rotations are the inverse of coordinate rotations. It can also be shown
(see Reference [02] for example) that extrinsic rotations are
equivalent to intrinsic rotations performed in the opposite order. So we can
show the following equivalence which allows users to input Euler angles using
the inertial navigation and automotive conventions directly with the Rotation
class.
import pprint
import numpy as np
from inertialsim.geometry import CoordinateRotation, Rotation
# Define some arbitrary angles
yaw = -np.deg2rad(60.0)
pitch = np.deg2rad(20)
roll = np.deg2rad(135)
# Inertial navigation and automotive convention
# Coordinate rotation from navigation to body frame
Cnb = CoordinateRotation.from_euler([yaw, pitch, roll], sequence="ZYX")
# Attitude = coordinate rotation from body to navigation frame
Cbn = Cnb.inverse()
# Robotics and math convention
# Attitude = rotation from navigation to body frame
R = Rotation.from_euler([roll, pitch, yaw], sequence="XYZ")
print("Cbn = ")
pprint.pprint(Cbn.as_matrix())
print("R = ")
pprint.pprint(R.as_matrix())
Cbn =
array([[[ 0.46984631, -0.49145005, -0.73329482],
[-0.81379768, -0.5629971 , -0.14410968],
[-0.34202014, 0.66446302, -0.66446302]]])
R =
array([[[ 0.46984631, -0.49145005, -0.73329482],
[-0.81379768, -0.5629971 , -0.14410968],
[-0.34202014, 0.66446302, -0.66446302]]])
Rigid transform conventions¶
Analogizing from the Pose conventions above, a rigid transform that transforms
points between a from frame and a to frame has the following components: