Robotics Forward Kinematics

This article describes the related information of robot kinematics, including the solution of rotation matrix, vector and mechanical arm pose, and using C# to describe the relevant algorithm.

Rotation Matrix

The rotation matrix in linear algebra is used to make the vector rotate in the Euler space.
In robotics, the rotation matrix is used to solve the posture (orientation) of the robot joint. In three-dimensional space, the eigenvalue of the rotation matrix is 1.

The method to calculate the rotation matrix

When calculating the rotation matrix, it is usually decomposed into three rotating matrices in X Y Z axis. In the right-hand Cartesian coordinates, the rotation direction is assumed to be the right-handed helix direction. The rotation matrix of each axis is shown as followed

X-rotation (Roll)
${\mathcal {R}}_{x}({\theta} _{x})= {\begin{bmatrix} 1&0&0 \\0 & \cos {\theta _{x}}&-\sin {\theta _{x}} \\ 0&\sin {\theta _{x}}&\cos {\theta _{x}}\end{bmatrix}}=\exp \left({\begin{bmatrix}0&0&0\\0&0&-\theta _{x}\\0&\theta _{x} & 0 \end{bmatrix}}\right)$
Y-rotation (Pitch)
${\mathcal {R}}_{y}(\theta _{y})={\begin{bmatrix}\cos {\theta _{y}}&0&\sin {\theta _{y}}\\0&1&0\\-\sin {\theta _{y}}&0&\cos {\theta _{y}}\end{bmatrix}}=\exp \left({\begin{bmatrix}0&0&\theta _{y}\\0&0&0\\-\theta _{y}&0&0\end{bmatrix}}\right)$
Z-rotation (Yaw)
${\mathcal {R}}_{z}(\theta _{z})={\begin{bmatrix}\cos {\theta _{z}}&-\sin {\theta _{z}}&0\\\sin {\theta _{z}}&\cos {\theta _{z}}&0\\0&0&1\end{bmatrix}}=\exp \left({\begin{bmatrix}0&-\theta _{z}&0\\\theta _{z}&0&0\\0&0&0\end{bmatrix}}\right)$

To calculate the final rotation matrix of the rigid body, multiply three rotation matrices.Then we can get ${\mathcal {R}}={\mathcal {R}}_{z}(\theta _{z})\,{\mathcal {R}}_{y}(\theta _{y})\,{\mathcal {R}}_{x}(\theta _{x})$

Conversion between the rotation matrix and the Euler Angle.

The euler Angle here refers to the order x-y-z (Tait–Bryan angles) instead of the traditional Euler angle order z-x-z. In order to distinguish between classical Euler angles, here we use${\theta}_{x},{\theta}_{y},{\theta}_{z}$ to indicate the angles.

Convert Euler Angle to the rotation matrix
As mentioned in the previous section, the rotation matrix can be obtained by multiplying three rotating matrices, so the equation of euler Angle to the rotation matrix is
${\mathbf{R}} = {\begin{bmatrix} 1&0&0 \\0 & \cos {\theta _{x}}&-\sin {\theta _{x}} \\ 0&\sin {\theta _{x}}&\cos {\theta _{x}}\end{bmatrix}}{\begin{bmatrix}\cos {\theta _{y}}&0&\sin {\theta _{y}}\\0&1&0\\-\sin {\theta _{y}}&0&\cos {\theta _{y}}\end{bmatrix}}{\begin{bmatrix}\cos {\theta _{z}}&-\sin {\theta _{z}}&0\\\sin {\theta _{z}}&\cos {\theta _{z}}&0\\0&0&1\end{bmatrix}}$
Here we define
${s}_{x} = \sin {{\theta}_{x}}; {c}_{x} = \cos {{\theta}_{x}}; {s}_{y} = \sin {{\theta}_{y}}; {c}_{y} = \cos {{\theta}_{y}}; {s}_{z} = \sin {{\theta}_{z}}; {c}_{z} = \cos {{\theta}_{z}}$
As a result
${\mathbf{R}} = {\begin{bmatrix} {c_y}{c_z} & {c_z}{s_x}{s_y} – {c_x}{s_z} & {s_x}{s_z} + {c_x}{c_z} {s_y} \\ {c_y}{s_z} & {c_x}{c_z}+{s_x}{s_y}{s_z} & {c_x}{s_y}{s_z}-{s_x}{c_z} \\ -{s_y} & {c_y}{s_x} & {c_x}{c_y} \end{bmatrix}}$

[codesyntax lang=”csharp” lines=”normal” title=”View the C# code here” blockstate=”collapsed”]

/// <summary>
/// Convert the Euler Angle to the Rotation Matrix
/// </summary>
/// <param name="euler">Euler Angle Vector</param>
/// <returns>Rotation Matrix</returns>
public static Matrix3F Euler2RMatrix(Vector3F euler)
{
    double sx = Math.Sin(euler.X), cx = Math.Cos(euler.X),
        sy = Math.Sin(euler.Y), cy = Math.Cos(euler.Y),
        sz = Math.Sin(euler.Z), cz = Math.Cos(euler.Z);
        return new Matrix3F(
            cy * cz, cz * sx * sy - cx * sz, sx * sz + cx * cz * sy,
            cy * sz, cx * cz + sx * sy * sz, cx * sy * sz - sx * cz,
            -sy,     cy * sx,                cx * cy
        );
}

[/codesyntax]

Convert the rotation matrix to the Euler Angle
Assume the rotation matrix is
$\mathbf{R} = {\begin{bmatrix} {r_{11}} & {r_{12}} & {r_{13}} \\ {r_{21}} & {r_{22}} & {r_{23}} \\ {r_{31}} & {r_{32}} & {r_{33}} \end{bmatrix}} $
Solve the equation mentioned in the last section we can get:
${{\theta}_{x}} = \operatorname{atan2}({r_{32}},{r_{33}})$
${{\theta}_{y}} = \operatorname{atan2}(-{r_{31}},\sqrt{{r_{32}^2}+{r_{33}^2}})$
${{\theta}_{y}} = \operatorname{atan2}({r_{21}},{r_{11}})$

atan2 is the function whose value range is $\left ( -\pi,\pi \right ] $ with the definition below:
$
\operatorname{atan2}(y, x) = \begin{cases}
\arctan\left(\frac y x\right) & \qquad x > 0 \\
\arctan\left(\frac y x\right) + \pi& \qquad y \ge 0 , x < 0 \\
\arctan\left(\frac y x\right) – \pi& \qquad y < 0 , x < 0 \\ +\frac{\pi}{2} & \qquad y > 0 , x = 0 \\
-\frac{\pi}{2} & \qquad y < 0 , x = 0 \\
\text{undefined} & \qquad y = 0, x = 0
\end{cases}$
See the Wikipedia for detail information

[codesyntax lang=”csharp” lines=”normal” title=”View the C# code for this section” blockstate=”collapsed”]

/// <summary>
/// Convert the rotation matrix to the euler angle
/// </summary>
/// <param name="rmat">The roatation matrix</param>
/// <returns>The Euler Angle vector</returns>
public static Vector3F RMatrix2Euler(Matrix3F rmat)
{
    return new Vector3F(
        Math.Atan2(rmat[2, 1], rmat[2, 2]),
        Math.Atan2(-rmat[2, 0], Math.Sqrt(rmat[2, 1] * rmat[2, 1] + rmat[2, 2] * rmat[2, 2])),
        Math.Atan2(rmat[1, 0], rmat[0, 0]));
}

[/codesyntax]

Rotation Matrix for Multiple Rotation

In robotics, the vector is usually rotated several times in order to solve the position of the manipulator. In general, such a rotation is a relative rotation to the previous one. In this case, the new rotation matrix can be obtained by multiplying the rotation matrix to the right of the original rotation matrix.

Assume the rotation matrix relative to previous joint is $\mathbf{R}$, The rotation matrix of the previous node relative to the base is ${\mathbf{R}}_{i-1}$, then the rotation matrix of the current node relative to the base frame. $ {\mathbf{R}}_{i}$ is:
$$ {\mathbf{R}}_{i} = {{\mathbf{R}}_{i-1}}{\mathbf{R}} $$

Joint and Endpoint Pose

In order to solve the position of the manipulator, the model of the manipulator is simplified to addition of the vectors. The end position can be obtained by adding up from the bottom up.
Assume the position of each joint is $ {\mathbf{P}}_{i} = {\begin{bmatrix} x \\ y \\ z \end{bmatrix}} $, the link length is${L}_{i}$

For each joint the pose can be calculated by

Define the initial state of the manipulator.
Calculate the rotation matrix in the initial state.${\mathbf{B}}_{i}$If all the joints facing the same direction, then${\mathbf{B}}_{i} = {\begin{bmatrix} 1&0&0 \\ 0&1&0 \\ 0&0&1 \end{bmatrix}}$
Calculate the relative rotation matrix in movement ${\mathbf{A}}_{i}$
Use the following formula to calculate the rotation matrix of the current joint relative to the base.
$$ {\mathbf{R}}_{i} = ({\mathbf{X}}_{i},{\mathbf{Y}}_{i},{\mathbf{Z}}_{i}) = {\mathbf{B}_{0}}{\mathbf{A}_{1}}{\mathbf{B}_{1}}{\mathbf{A}_{2}} \cdots {\mathbf{B}_{i-1}} {\mathbf{A}_{i}} $$
The current joint position is
$$ {\mathbf{P}}_{i} = {\mathbf{P}}_{i-1} + {\mathbf{R}}_{i} * ({{L}_{i}}*{{\mathbf{Z}}_{0}}) $$
The euler Angle of the endpoint can be obtained through the formula of the first section.

Reference

Wikipedia Author, “Rotation matrix,” 18 2 2018. https://en.wikipedia.org/wiki/Rotation_matrix
高濑国克, “マニピュレータの基礎理論,” 1983.
“旋转矩阵、欧拉角、四元数理论及其转换关系,” 21 5 2017. http://blog.csdn.net/lql0716/article/details/72597719.

Frozen Code