Rigid-Body Motions
Textbook Chapter 3
ModernRoboticsBook.near_zero — Method
near_zero(z)Determines whether a scalar is small enough to be treated as zero.
Arguments
z: a scalar value.
Returns
true if z is approximately zero (within absolute tolerance $10^{-6}$); false otherwise.
Examples
julia> near_zero(-1e-7)
trueModernRoboticsBook.adjoint_representation — Method
adjoint_representation(T)Computes the adjoint representation of a homogeneous transformation matrix.
The adjoint map transforms twists (spatial velocities) between reference frames. Given a twist $V$ expressed in one frame, $[\text{Ad}_T] V$ gives the same physical motion expressed in the frame related by $T$. This is used throughout robotics to transform Jacobians and wrenches between body and space frames.
Arguments
T: a $4 \times 4$ homogeneous transformation matrix.
Returns
The $6 \times 6$ adjoint representation $[\text{Ad}_T]$.
Examples
julia> adjoint_representation([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])
6×6 StaticArraysCore.SMatrix{6, 6, Int64, 36} with indices SOneTo(6)×SOneTo(6):
1 0 0 0 0 0
0 0 -1 0 0 0
0 1 0 0 0 0
0 0 3 1 0 0
3 0 0 0 0 -1
0 0 0 0 1 0ModernRoboticsBook.axis_angle3 — Method
axis_angle3(expc3)Converts a 3-vector of exponential coordinates for rotation into axis-angle form.
Arguments
expc3: a 3-vector of exponential coordinates for rotation $\hat{\omega}\theta$.
Returns
A tuple (ω̂, θ) of the unit rotation axis and the rotation angle.
Examples
julia> axis_angle3([1, 2, 3])
([0.2672612419124244, 0.5345224838248488, 0.8017837257372732], 3.7416573867739413)ModernRoboticsBook.axis_angle6 — Method
axis_angle6(expc6)Converts a 6-vector of exponential coordinates into screw axis-angle form.
Arguments
expc6: a 6-vector of exponential coordinates $\mathcal{S}\theta$.
Returns
A tuple (S, θ) of the screw axis and the distance travelled along the axis.
Examples
julia> axis_angle6([1, 0, 0, 1, 2, 3])
([1.0, 0.0, 0.0, 1.0, 2.0, 3.0], 1.0)ModernRoboticsBook.distance_to_se3 — Method
distance_to_se3(mat)Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold.
Arguments
mat: a $4 \times 4$ matrix.
Returns
The Frobenius distance from SE(3), or $10^9$ if $\det(R) < 0$.
Examples
julia> distance_to_se3([1.0 0.0 0.0 1.2; 0.0 0.1 -0.95 1.5; 0.0 1.0 0.1 -0.9; 0.0 0.0 0.1 0.98])
0.13493053768513638ModernRoboticsBook.distance_to_so3 — Method
distance_to_so3(mat)Returns the Frobenius norm to describe the distance of mat from the SO(3) manifold.
Arguments
mat: a $3 \times 3$ matrix.
Returns
The Frobenius norm of $R^T R - I$, or $10^9$ if $\det(R) < 0$.
Examples
julia> distance_to_so3([1.0 0.0 0.0; 0.0 0.1 -0.95; 0.0 1.0 0.1])
0.08835298523536149ModernRoboticsBook.is_se3 — Method
is_se3(mat)Returns true if mat is close to or on the manifold SE(3).
Arguments
mat: a $4 \times 4$ matrix.
Returns
true if mat is close to SE(3); false otherwise.
Examples
julia> is_se3([1.0 0.0 0.0 1.2; 0.0 0.1 -0.95 1.5; 0.0 1.0 0.1 -0.9; 0.0 0.0 0.1 0.98])
falseModernRoboticsBook.is_so3 — Method
is_so3(mat)Returns true if mat is close to or on the manifold SO(3).
Arguments
mat: a $3 \times 3$ matrix.
Returns
true if mat is close to SO(3); false otherwise.
Examples
julia> is_so3([1.0 0.0 0.0; 0.0 0.1 -0.95; 0.0 1.0 0.1])
falseModernRoboticsBook.matrix_exp3 — Method
matrix_exp3(so3mat)Computes the matrix exponential of a matrix in so(3) using Rodrigues' formula.
The matrix exponential maps elements of a Lie algebra (infinitesimal motions) to the corresponding Lie group (finite motions). For rotations, it converts an angular velocity $[\omega]\theta$ in so(3) into the actual rotation matrix $R$ in SO(3) that results from rotating by angle $\theta$ about axis $\hat{\omega}$. This is the mathematical foundation of the product-of-exponentials formula for robot kinematics.
Arguments
so3mat: a $3 \times 3$ skew-symmetric matrix in so(3).
Returns
The corresponding rotation matrix $R$ in SO(3).
Examples
julia> matrix_exp3([0 -3 2; 3 0 -1; -2 1 0])
3×3 StaticArraysCore.SMatrix{3, 3, Float64, 9} with indices SOneTo(3)×SOneTo(3):
-0.694921 0.713521 0.0892929
-0.192007 -0.303785 0.933192
0.692978 0.63135 0.348107ModernRoboticsBook.matrix_exp6 — Method
matrix_exp6(se3mat)Computes the matrix exponential of an se(3) representation of exponential coordinates.
This is the SE(3) version of matrix_exp3. It converts a twist $[\mathcal{S}]\theta$ in se(3) into the rigid-body transformation $T$ in SE(3) that results from following that screw motion. This is how each joint's contribution is computed in the product-of-exponentials formula for forward kinematics.
Arguments
se3mat: a $4 \times 4$ matrix in se(3).
Returns
The corresponding homogeneous transformation matrix $T$ in SE(3).
Examples
julia> matrix_exp6([0 0 0 0; 0 0 -1.57079632 2.35619449; 0 1.57079632 0 2.35619449; 0 0 0 0])
4×4 StaticArraysCore.SMatrix{4, 4, Float64, 16} with indices SOneTo(4)×SOneTo(4):
1.0 0.0 0.0 0.0
0.0 6.7949e-9 -1.0 1.01923e-8
0.0 1.0 6.7949e-9 3.0
0.0 0.0 0.0 1.0ModernRoboticsBook.matrix_log3 — Method
matrix_log3(R)Computes the matrix logarithm of a rotation matrix.
The matrix logarithm is the inverse of the matrix exponential. It extracts the angular velocity $[\omega]\theta$ from a rotation matrix $R$, telling you the axis and angle of rotation that produced $R$. This is essential for error computation in iterative algorithms like inverse kinematics.
Arguments
R: a rotation matrix in SO(3).
Returns
The matrix logarithm of R in so(3).
Examples
julia> matrix_log3([0 0 1; 1 0 0; 0 1 0])
3×3 Matrix{Float64}:
0.0 -1.2092 1.2092
1.2092 0.0 -1.2092
-1.2092 1.2092 0.0 ModernRoboticsBook.matrix_log6 — Method
matrix_log6(T)Computes the matrix logarithm of a homogeneous transformation matrix.
This is the SE(3) version of matrix_log3. It extracts the twist $[\mathcal{S}]\theta$ from a transformation $T$, recovering the screw motion that produced it. This is used in inverse kinematics to compute the body twist error between the current and desired end-effector configurations.
Arguments
T: a $4 \times 4$ homogeneous transformation matrix in SE(3).
Returns
The $4 \times 4$ matrix logarithm $[\mathcal{S}\theta]$ in se(3).
Examples
julia> matrix_log6([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])
4×4 StaticArraysCore.SMatrix{4, 4, Float64, 16} with indices SOneTo(4)×SOneTo(4):
0.0 0.0 0.0 0.0
0.0 0.0 -1.5708 2.35619
0.0 1.5708 0.0 2.35619
0.0 0.0 0.0 0.0ModernRoboticsBook.project_to_se3 — Method
project_to_se3(mat)Returns a projection of mat into SE(3).
Similar to project_to_so3, this corrects numerical drift in homogeneous transformation matrices. After many multiplications, the rotation part may no longer be orthogonal. This projects the matrix back onto SE(3) to maintain valid rigid-body transformations.
Arguments
mat: a $4 \times 4$ matrix near SE(3).
Returns
The closest homogeneous transformation matrix in SE(3).
Examples
julia> project_to_se3([0.675 0.150 0.720 1.2; 0.370 0.771 -0.511 5.4; -0.630 0.619 0.472 3.6; 0.003 0.002 0.010 0.9])
4×4 StaticArraysCore.SMatrix{4, 4, Float64, 16} with indices SOneTo(4)×SOneTo(4):
0.679011 0.148945 0.718859 1.2
0.373207 0.773196 -0.512723 5.4
-0.632187 0.616428 0.469421 3.6
0.0 0.0 0.0 1.0ModernRoboticsBook.project_to_so3 — Method
project_to_so3(mat)Returns a projection of mat into SO(3).
Numerical computations (e.g. repeated matrix multiplications) can cause rotation matrices to drift away from SO(3) — they may no longer be exactly orthogonal with determinant 1. This function finds the closest valid rotation matrix using SVD, which is essential for maintaining physical consistency in long-running simulations.
Arguments
mat: a $3 \times 3$ matrix near SO(3).
Returns
The closest rotation matrix in SO(3), computed using SVD projection.
Examples
julia> project_to_so3([0.675 0.150 0.720; 0.370 0.771 -0.511; -0.630 0.619 0.472])
3×3 Matrix{Float64}:
0.679011 0.148945 0.718859
0.373207 0.773196 -0.512723
-0.632187 0.616428 0.469421ModernRoboticsBook.rotation_position_to_transform — Method
rotation_position_to_transform(R, p)Converts a rotation matrix and a position vector into homogeneous transformation matrix.
Arguments
R: a $3 \times 3$ rotation matrix.p: a 3-vector position.
Returns
The corresponding $4 \times 4$ homogeneous transformation matrix $T$.
Examples
julia> rotation_position_to_transform([1 0 0; 0 0 -1; 0 1 0], [1, 2, 5])
4×4 StaticArraysCore.SMatrix{4, 4, Float64, 16} with indices SOneTo(4)×SOneTo(4):
1.0 0.0 0.0 1.0
0.0 0.0 -1.0 2.0
0.0 1.0 0.0 5.0
0.0 0.0 0.0 1.0ModernRoboticsBook.screw_to_axis — Method
screw_to_axis(q, s, h)Takes a parametric description of a screw axis and converts it to a normalized screw axis.
A screw axis describes any rigid-body motion as a rotation about an axis combined with a translation along that axis (like a corkscrew). It is defined by a point $q$ on the axis, a direction $s$, and a pitch $h$ (ratio of linear to angular motion). When $h = 0$ the motion is pure rotation; when $h = \infty$ it is pure translation. Every joint in a robot can be described by a screw axis.
Arguments
q: a point on the screw axis.s: the unit direction vector of the screw axis.h: the pitch of the screw axis.
Returns
The normalized 6-vector screw axis $\mathcal{S}$.
Examples
julia> screw_to_axis([3; 0; 0], [0; 0; 1], 2)
6-element StaticArraysCore.SVector{6, Int64} with indices SOneTo(6):
0
0
1
0
-3
2ModernRoboticsBook.se3_to_vec — Method
se3_to_vec(se3mat)Converts an se3 matrix into a spatial velocity vector.
Arguments
se3mat: a $4 \times 4$ matrix in se(3).
Returns
The corresponding 6-vector twist (angular velocity, linear velocity).
Examples
julia> se3_to_vec([0 -3 2 4; 3 0 -1 5; -2 1 0 6; 0 0 0 0])
6-element StaticArraysCore.SVector{6, Int64} with indices SOneTo(6):
1
2
3
4
5
6ModernRoboticsBook.so3_to_vec — Method
so3_to_vec(so3mat)Converts an so(3) representation to a 3-vector.
Arguments
so3mat: a $3 \times 3$ skew-symmetric matrix in so(3).
Returns
The corresponding 3-vector of angular velocities.
Examples
julia> so3_to_vec([0 -3 2; 3 0 -1; -2 1 0])
3-element StaticArraysCore.SVector{3, Int64} with indices SOneTo(3):
1
2
3ModernRoboticsBook.transform_inv — Method
transform_inv(T)Inverts a homogeneous transformation matrix.
Arguments
T: a $4 \times 4$ homogeneous transformation matrix.
Returns
The inverse $T^{-1}$, computed using $R^T$.
Examples
julia> transform_inv([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])
4×4 StaticArraysCore.SMatrix{4, 4, Float64, 16} with indices SOneTo(4)×SOneTo(4):
1.0 0.0 0.0 0.0
0.0 0.0 1.0 -3.0
0.0 -1.0 0.0 0.0
0.0 0.0 0.0 1.0ModernRoboticsBook.transform_to_rotation_position — Method
transform_to_rotation_position(T)Converts a homogeneous transformation matrix into a rotation matrix and position vector.
Arguments
T: a $4 \times 4$ homogeneous transformation matrix.
Returns
A tuple (R, p) of the rotation matrix and position vector.
Examples
julia> transform_to_rotation_position([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])
([1 0 0; 0 0 -1; 0 1 0], [0, 0, 3])ModernRoboticsBook.vec_to_se3 — Method
vec_to_se3(V)Converts a spatial velocity vector into a 4x4 matrix in se(3).
se(3) is the space of $4 \times 4$ matrices that represent twists — combined angular and linear velocities of a rigid body. It is the Lie algebra of the transformation group SE(3). A twist $V = (\omega, v)$ encodes both rotation (angular velocity $\omega$) and translation (linear velocity $v$) in a single object, which is key to the product-of-exponentials formula for kinematics.
Arguments
V: a 6-vector spatial velocity (angular velocity, linear velocity).
Returns
The corresponding $4 \times 4$ matrix in se(3).
Examples
julia> vec_to_se3([1, 2, 3, 4, 5, 6])
4×4 StaticArraysCore.SMatrix{4, 4, Float64, 16} with indices SOneTo(4)×SOneTo(4):
0.0 -3.0 2.0 4.0
3.0 0.0 -1.0 5.0
-2.0 1.0 0.0 6.0
0.0 0.0 0.0 0.0ModernRoboticsBook.vec_to_so3 — Method
vec_to_so3(ω)Converts a 3-vector to an so(3) representation.
so(3) is the space of all $3 \times 3$ skew-symmetric matrices. Each element represents an angular velocity. If $\omega$ is the angular velocity vector, then $[\omega]$ (its skew-symmetric form) can be used to compute cross products as matrix multiplications: $\omega \times v = [\omega] v$. This is the Lie algebra of the rotation group SO(3).
Arguments
ω: a 3-vector of angular velocities.
Returns
The corresponding $3 \times 3$ skew-symmetric matrix in so(3).
Examples
julia> vec_to_so3([1, 2, 3])
3×3 StaticArraysCore.SMatrix{3, 3, Int64, 9} with indices SOneTo(3)×SOneTo(3):
0 -3 2
3 0 -1
-2 1 0