Coordinate System Translation, Rotation, and Position Offset: Basic Principles and Application Examples

Basic Principles

Basic Definitions

Coordinate System: A reference frame defined by an origin and a set of mutually orthogonal, oriented basis axes (usually X, Y, Z), used to uniquely represent the position and orientation of a point in space under specific units.

• 2D contains two axes (X, Y); 3D contains three axes (X, Y, Z);
• It is necessary to agree on chirality (usually the right-hand system) and units (mm, m, etc.);
• Vectors and orientations given relative to the same coordinate system are comparable and composable.

Common Types of Coordinate Transformations

Translation: poseAdd()

• Meaning: Move the position of the same point within the same coordinate system, with its orientation unchanged.
• Formula (with translation when a point expressed in frame B is transformed to frame A):

$$ \mathbf{p}{\mathrm{A}} = \mathbf{R}{\mathrm{A}}^{\mathrm{B}},\mathbf{p}{\mathrm{B}}+ \mathbf{t}{\mathrm{A}}^{\mathrm{B}} $$

Here $\mathbf t_A^B=[t_x,t_y,t_z]^T$ is the translation vector expressed in coordinate system A.

• $\mathbf R$ is responsible for "changing direction", and $\mathbf t$ is responsible for "adding position". For translation only, $\mathbf p_A=\mathbf p_B+\mathbf t$.

Rotation: poseRotation()

• Meaning: Rotate the orientation within the same coordinate system; the position changes in a circular path relative to the origin.

• Formula: $\mathbf p'_A=\mathbf R,\mathbf p_A$, where $\mathbf R\in SO(3)$, satisfying $\mathbf R^T\mathbf R=\mathbf I$ and $\det\mathbf R=+1$.

• Common representations (3D):

  • Euler angles ZYX (yaw-pitch-roll): $\mathbf R=\mathbf R_z(\psi)\mathbf R_y(\theta)\mathbf R_x(\phi)$
  • Axis-angle (rotation by angle $\theta$ about the unit axis $\hat\omega$): Rodrigues' formula $\mathbf{R}=\mathbf{I}+\sin\theta,\lbrack\hat{\omega}\rbrack_\times+(1-\cos\theta),(\lbrack\hat{\omega}\rbrack_\times)^2$

Translation + Rotation Combined (Pose / Homogeneous Transformation): poseTrans()

Pack rotation and translation into a 4×4 matrix:

$$ \mathbf T= \begin{bmatrix} \mathbf R & \mathbf t\ \mathbf 0^T & 1 \end{bmatrix},\quad \begin{bmatrix}\mathbf p_A\1\end{bmatrix}=\mathbf T_A^B \begin{bmatrix}\mathbf p_B\1\end{bmatrix} $$

• Composition order: Right-multiply by the transformation that is closer to the point. For example, first offset/rotate within the TCP's own frame ($\Delta \mathbf T_{tcp}$), then place it back into the base:

$$ \mathbf{T}{\mathrm{base}}^{\mathrm{tcp,new}} = \mathbf{T}{\mathrm{base}}^{\mathrm{tcp}} ,\Delta \mathbf{T}_{\mathrm{tcp}} $$

• Inverse transformation poseInverse(): Convert the "pose from B to A" into the "pose from A to B",

$$ \mathbf{T}^{-1} = \begin{pmatrix} \mathbf{R}^{\mathsf{T}} & -,\mathbf{R}^{\mathsf{T}}\mathbf{t}\ \mathbf{0} & 1 \end{pmatrix} $$

Application Examples

1. Point Offset

Example program package

1. Point offset via the Direction-Until node

   

2. Feature in the 0.31 branch: point offset relative to a variable or waypoint

   

2. Coordinate System Offset

1. Implement offset via a custom coordinate system

   

3. Coordinate System + Point Offset