9.6.1. 2D angles

At the end of the example in section Kinematic chains, we calculated the frames that express the orientation of both the upper arm (\(^\text{global}_\text{upper arm} T\)) and forearm (\(^\text{global}_\text{forearm} T\)) in global coordinates.

Let’s see how we can extract the elbow flexion angle from those two frames.

The first step is to calculate the homogeneous transform from the upper arm to the forearm. This is the same as expressing the forearm frame in reference to the upper arm frame.

import kineticstoolkit.lab as ktk
import numpy as np

# Upper arm and forearm frames, expressed in global coordinates
# (calculated in the last example):
global_T_upperarm = np.array(
    [
        [
            [0.8660254, -0.5, 0.0, 0.15],
            [0.5, 0.8660254, 0.0, 0.7],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ]
    ]
)

global_T_forearm = np.array(
    [
        [
            [0.64278761, -0.76604444, 0.0, 0.34],
            [0.76604444, 0.64278761, 0.0, 0.37091035],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ]
    ]
)

# Express the forearm frame in local upper arm coordinates
upperarm_T_forearm = ktk.geometry.get_local_coordinates(
    global_T_forearm, global_T_upperarm
)

upperarm_T_forearm

array([[[ 0.93969262, -0.34202014,  0.        ,  0.        ],
        [ 0.34202014,  0.93969262,  0.        , -0.38      ],
        [ 0.        ,  0.        ,  1.        ,  0.        ],
        [ 0.        ,  0.        ,  0.        ,  1.        ]]])

In section Geometry basics: coordinate systems, points, vectors, and transforms, we expressed the generic form of a 2D frame in the xy plane:

\[\begin{split} ~^\text{upper arm}_\text{forearm}T = \begin{bmatrix} \cos(\theta) & -\sin(\theta) & 0 & x \\ \sin(\theta) & \cos(\theta) & 0 & y \\ 0 & 0 & 1 & z \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{split}\]

Here, \(\theta\) corresponds to the flexion angle of the elbow. To extract θ from the resulting \(^\text{upper arm}_\text{forearm}T\) matrix, we use simple trigonometric relations:

\[ \tan(\theta) = \frac{\sin(\theta)}{\cos(\theta)} = \frac{T_{21}}{T_{11}} = \frac{0.34202}{0.93970} \]

\[\begin{split} \theta = \text{atan2}(y=0.93970, x=0.36396) \\= 20^\circ \end{split}\]

We can also use Kinetics Toolkit’s function ktk.geometry.get_angles to extract this angle automatically:

elbow_angles = ktk.geometry.get_angles(
    upperarm_T_forearm, seq="xyz", degrees=True
)

elbow_angles

array([[ 0.        ,  0.        , 19.99999971]])