9.3.3. Moving frames (transforms)

The orientation and position of a frame (defined as a coordinate system attached to a segment) are expressed by a homogeneous transform. We remind that the first three columns are three vectors (the direction of three axes), and the fourth column is a point (the position of the origin). Therefore, since a homogeneous transform can move both points and vectors, it can also move complete frames.

Fig. 9.12 Rotating and translating a local frame in respect to the global reference frame.

Let’s use the same homogeneous transform to rotate and translate the frame \(^\text{global} _\text{local-initial} F\) (which reads as Frame ‘local’ in its initial pose, expressed in global coordinates) as shown in Fig. 9.12.

\[ ^\text{global} _\text{local-tranformed} F ~~~ = ~~~ T ~~~ ^\text{global} _\text{local-initial} F \]

We first express the unrotated frame \(^\text{global} _\text{local-initial} F\):

\[\begin{split} ^\text{global} _\text{local-initial} F = \begin{bmatrix} \cos(0) & -\sin(0) & 0 & 10 \\ \sin(0) & \cos(0) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 10 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{split}\]

The transformed frame is calculated using:

\[\begin{split} ^\text{global} _\text{local-transformed} F = \begin{bmatrix} \cos(30) & -\sin(30) & 0 & 2 \\ \sin(30) & \cos(30) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 10 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\= \begin{bmatrix} \cos(30) & -\sin(30) & 0 & 10.66 \\ \sin(30) & \cos(30) & 0 & 5 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{split}\]

9.3.3.1. Application in Kinetics Toolkit

The transform \(T\) is created using:

import kineticstoolkit.lab as ktk
import numpy as np

T = ktk.geometry.create_transform_series(
    angles=[30],
    degrees=True,
    seq="z",  # Which means a rotation around the z axis
    positions=[[2.0, 0.0, 0.0, 1.0]],
)

T

array([[[ 0.8660254, -0.5      ,  0.       ,  2.       ],
        [ 0.5      ,  0.8660254,  0.       ,  0.       ],
        [ 0.       ,  0.       ,  1.       ,  0.       ],
        [ 0.       ,  0.       ,  0.       ,  1.       ]]])

The initial local frame is expressed as:

initial_frame = np.array(
    [
        [
            [1.0, 0.0, 0.0, 10.0],
            [0.0, 1.0, 0.0, 0.0],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ]
    ]
)

The rotated frame is:

ktk.geometry.matmul(T, initial_frame)

array([[[ 0.8660254 , -0.5       ,  0.        , 10.66025404],
        [ 0.5       ,  0.8660254 ,  0.        ,  5.        ],
        [ 0.        ,  0.        ,  1.        ,  0.        ],
        [ 0.        ,  0.        ,  0.        ,  1.        ]]])

9.3.3.2. Direct transformation in Kinetics Toolkit

We can also rotate and translate the initial frame using ktk.geometry.rotate and ktk.geometry.translate:

rotated_frame = ktk.geometry.rotate(
    initial_frame, seq="z", angles=[30], degrees=True
)
final_frame = ktk.geometry.translate(rotated_frame, [[2.0, 0.0, 0.0]])

final_frame

array([[[ 0.8660254 , -0.5       ,  0.        , 10.66025404],
        [ 0.5       ,  0.8660254 ,  0.        ,  5.        ],
        [ 0.        ,  0.        ,  1.        ,  0.        ],
        [ 0.        ,  0.        ,  0.        ,  1.        ]]])