Robotic Arm Inverse Kinematics

Programming the inverse kinematics for a six axis industrial serial manipulator with a ortho-paralell basis and spherical wrist.

Jan 16, 2024

Markdown

Updated Jan 16, 2024

Introduction

Inverse kinematics refers to the problem of determining the angle of each joint in a robotic system given target position and or orientation. In this case there are six joints \(\theta_1, ..., \theta_6\) and the target is a position vector and orientation rotation matrix.

This post is based on the conference paper “An Analytical Solution of the Inverse Kinematics Problem of Industrial Serial Manipulators with an Ortho-parallel Basis and a Spherical Wrist” by Mathias Brandstötter, Michael Hofbaur and Arthur Angerer. The paper is available to download at researchgate.net/publication/264212870.

The post will briefly cover the mathematics and setup of the robotic arm. Then give a example implementation written in ANSI C. The code only depends on #include <math.h>. The implementation computes all of the solutions on each function call.

The robotic arm spesified in the paper is a six axis robotic arm with an ortho-paralell basis and a spherical wrist. An ortho-paralell basis essentially means that the first three axes (the basis of the arm), are configured in the following way.

A spherical wrist meaning that the last three axes which are connected to the basis of the arm are configured in this fashion.

This configuration of a robotic arm is most common among the majority of industrial robotic arms.

Solutions

Given a robotic arm in a ortho-paralell + spherical wrist configuration one can use the following diagram to determine the 7 essential geometrical parameters of the robotic arm which determine the inverse kinematics of the arm. The parameters are: \(c_1\), \(c_2\), \(c_3\), \(c_4\), \(a_1\), \(a_2\) and \(b\).

The desired pose of the end-effector is spesified by a \(3 \times 1\) position vector \(u_0\) and a \(3 \times 3\) rotation matrix for the orientation \(R^0_e\).

\[ u_0 = \begin{bmatrix} u_{x0} \\ u_{y0} \\ u_{z0} \end{bmatrix} \]

\[ R^0_e = \begin{bmatrix} e_{1,1} & e_{1,2} & e_{1,3} \\ e_{2,1} & e_{2,2} & e_{2,3} \\ e_{3,1} & e_{3,2} & e_{3,3} \end{bmatrix} \]

The geometrical configuration of the arm and pose of the end-effector can be modelled in the following way.

typedef struct opw_parameters {
    double c1, c2, c3, c4;
    double a1, a2;
    double b;
} opw_t;

typedef struct vec3 { double x, y, z; } vec3_t;

typedef struct rotation_matrix {
    double r11, r12, r13;
    double r21, r22, r23;
    double r31, r32, r33;
} rotm_t;

Positioning

The first three angles \(\theta_1\), \(\theta_2\) and \(\theta_3\) are determined by calculating the final position of the base of the arm which yields the first three angles. The end-effector position for the base is given by.

\[ \begin{bmatrix} c_{x0} \\ c_{y0} \\ c_{z0} \end{bmatrix} = \begin{bmatrix} u_{x0} \\ u_{y0} \\ u_{z0} \end{bmatrix} - c_4 R_e^0 \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \]

Which then yields.

\[ \begin{aligned} \theta_1;i &= \text{atan2}(c_{y0}, c_{x0}) \\ &- atan2(b, n_{x1} + a_1) \\ \\ \theta_1;ii &= \text{atan2}(c_{y0}, c_{x0}) \\ &+ atan2(b, n_{x1} + a_1) - \pi \\ \\ \theta_2;i,ii &= \pm \text{acos} \left( \frac{s_1^2 + c_2^2 - k^2}{2 s_1 c_2} \right ) \\ &+ \text{atan2}(n_{x1}, c_{z0} - c_1) \\ \\ \theta_2;iii,iv &= \pm \text{acos} \left( \frac{s_1^2 + c_2^2 - k^2}{2 s_1 c_2} \right ) \\ &- \text{atan2}(n_{x1} + 2a_1, c_{z0} - c_1) \\ \\ \theta_3;i,ii &= \pm \text{acos} \left ( \frac{s_1^2 - c_2^2 - k^2}{2c_2k} \right ) \\ &- \text{atan2}(a_2, c_3) \\ \\ \theta_3;iii,iv &= \pm \text{acos} \left ( \frac{s_2^2 - c_2^2 - k^2}{2c_2k} \right ) \\ &- \text{atan2}(a_2, c_3) \end{aligned} \]

Where.

\[ \begin{aligned} n_{x1} &= \sqrt{c_{x0}^2 + c_{y0}^2 - b^2 - a_1} \\ s_1^2 &= n_{x1}^2 + (c_{z0} - c_1)^2 \\ s_2^2 &= (n_{x1} + 2a_1)^2 + (c_{z0} - c_1)^2 \\ k^2 &= a_2^2 + c_3^2 \end{aligned} \]

Orientation

\[ \begin{aligned} \theta_{4;p} &= \text{atan2}(e_{2,3} \mathfrak{c}_{1;p} - e_{1,3}\mathfrak{s}_{1;p}, \\ &e_{1,3}\mathfrak{c}_{23;p} \mathfrak{c}_{1;p} +e_{2,3}\mathfrak{23;p}\mathfrak{s}_{1;p}-e_{3,3}\mathfrak{s}_{23;p} ) \\ \\ \theta_{4;q} &= \theta_{4;p} + \pi \\ \\ \theta_{5;p} &= \text{atan2}\left(\sqrt{1-m_p^2}, m_p\right) \\ \\ \theta_{5;q} &= -\theta_{5;p} \\ \\ \theta_{6;p} &= \text{atan2}(e_{1,2}\mathfrak{s}_{23;p}\mathfrak{c}_{1;p} + e_{2,2}\mathfrak{s}_{23;p}\mathfrak{s}_{1;p} \\ &e_{3,2}\mathfrak{c}_{23;p}, -e_{1,1}\mathfrak{s}_{23;p}\mathfrak{c}_{1;p} - e_{2,1}\mathfrak{s}_{23;p}\mathfrak{s}_{1;p} \\ &-e_{3,1}\mathfrak{c}_{23;p}) \\ \\ \theta_{6;q} &= \theta_{6;p} - \pi \end{aligned} \]

Where.

\[ \begin{aligned} m_p &= e_{1,3}\mathfrak{s}_{23;p}\mathfrak{c}_{1;p} \\ &+ e_{2,3}\mathfrak{s}_{23;p}\mathfrak{s}_{1;p} \\ &+ e_{3,3}\mathfrak{c}_{23;p} \\ \\ \mathfrak{s}_{1;p} &= sin(\theta_{1;p}) \\ \\ \mathfrak{s}_{23;p} &= sin(\theta_{2;p} + \theta_{3;p}) \\ \\ \mathfrak{c}_{1;p} &= cos(\theta_{1;p}) \\ \\ \mathfrak{c}_{23;p} &= cos(\theta_{2;p} + \theta_{3;p}) \\ \\ p &= \{i, ii, iii, iv\} \\ \\ q &= \{v, vi, vii, viii\} \end{aligned} \]

All 8 Solutions

Each row in the following table is a solution to the inverse kinematics problem.

\(\theta_{1;i}\)	\(\theta_{2;i}\)	\(\theta_{3;i}\)	\(\theta_{4;i}\)	\(\theta_{5;i}\)	\(\theta_{6;i}\)
\(\theta_{1;i}\)	\(\theta_{2;ii}\)	\(\theta_{3;ii}\)	\(\theta_{4;ii}\)	\(\theta_{5;ii}\)	\(\theta_{6;ii}\)
\(\theta_{1;ii}\)	\(\theta_{2;iii}\)	\(\theta_{3;iii}\)	\(\theta_{4;iii}\)	\(\theta_{5;iii}\)	\(\theta_{6;iii}\)
\(\theta_{1;ii}\)	\(\theta_{2;iv}\)	\(\theta_{3;iv}\)	\(\theta_{4;iv}\)	\(\theta_{5;iv}\)	\(\theta_{6;iv}\)
\(\theta_{1;i}\)	\(\theta_{2;i}\)	\(\theta_{3;i}\)	\(\theta_{4;v}\)	\(\theta_{5;v}\)	\(\theta_{6;v}\)
\(\theta_{1;i}\)	\(\theta_{2;ii}\)	\(\theta_{3;ii}\)	\(\theta_{4;vi}\)	\(\theta_{5;vi}\)	\(\theta_{6;vi}\)
\(\theta_{1;ii}\)	\(\theta_{2;iii}\)	\(\theta_{3;iii}\)	\(\theta_{4;vii}\)	\(\theta_{5;vii}\)	\(\theta_{6;vii}\)
\(\theta_{1;ii}\)	\(\theta_{2;iv}\)	\(\theta_{3;iv}\)	\(\theta_{4;viii}\)	\(\theta_{5;viii}\)	\(\theta_{6;viii}\)

Implementation

The following code implements the mathematics which calculate the inverse kinematics. The function opw_inverse is the function which calculates all the solutions. It uses a helper function opw_theta6_correction which is used to recalculate \(\theta_6\) per solution since numerical errors might arise due to numbers being close to zero. This code comes with no guarantees so if you accidentally kill yourself or somebody else it’s not my fault :)

One concious design choice of the code that I will cover explicitly is that, the structs param, pos, and rot are passed by value, not by reference. I wrote it like this since all values are accessed, multiple times. Passing by reference does not make it faster since it would need to dereference each time. Copying the values makes more sense in this scenario. I am not afraid of pointers, I do not condone people using Rust (ever). This may seem contradictory to having the output array sols being accessed by reference, however. It is I guess. Anyway here is the code (works on my machine).

#define c1  (param.c1)
#define c2  (param.c2)
#define c3  (param.c3)
#define c4  (param.c4)
#define a1  (param.a1)
#define a2  (param.a2)
#define b   (param.b)
#define ux0 (pos.x)
#define uy0 (pos.y)
#define uz0 (pos.z)
#define e11 (rot.r11)
#define e12 (rot.r12)
#define e13 (rot.r13)
#define e21 (rot.r21)
#define e22 (rot.r22)
#define e23 (rot.r23)
#define e31 (rot.r31)
#define e32 (rot.r32)
#define e33 (rot.r33)

double opw_theta6_correction(double theta1, rotm_t rot) {
    double x, y, ux, uy;

    ux = -sin(theta1);
    uy = cos(theta1);
    x  = uy*e33*e11 + ux*e21 + e13*e31;
    y  = -ux*e33*e11 + uy*e21 + e23*e31;

    return atan2(y, x);
}

void opw_inverse(opw_t param, vec3_t pos, rotm_t rot, double sols[6*8]) {
    double cx0, cy0, cz0;
    double nx1, s1_sqr, s2_sqr, k_sqr, s1, s2, k;
    double tmp1, tmp2, tmp3, tmp4;
    double m_i, m_ii, m_iii, m_iv;
    double theta1_i, theta1_ii;
    double theta2_i, theta2_ii, theta2_iii, theta2_iv;
    double theta3_i, theta3_ii, theta3_iii, theta3_iv;
    double theta4_i, theta4_ii, theta4_iii, theta4_iv;
    double theta4_v, theta4_vi, theta4_vii, theta4_viii;
    double theta5_i, theta5_ii, theta5_iii, theta5_iv;
    double theta5_v, theta5_vi, theta5_vii, theta5_viii;
    double theta6_i, theta6_ii, theta6_iii, theta6_iv;
    double theta6_v, theta6_vi, theta6_vii, theta6_viii;
    double _s1[4], _c1[4], _s23[4], _c23[4];
    double zero_threshold;

    zero_threshold = 1e-6;

    cx0 = ux0 - c4*e13;
    cy0 = uy0 - c4*e23;
    cz0 = uz0 - c4*e33;

    nx1    = sqrt(cx0*cx0 + cy0*cy0 - b*b) - a1;
    s1_sqr = nx1*nx1 + (cz0-c1)*(cz0-c1);
    s2_sqr = (nx1+2*a1)*(nx1+2*a1) + (cz0-c1)*(cz0-c1);
    k_sqr  = a2*a2 + c3*c3;
    s1     = sqrt(s1_sqr);
    s2     = sqrt(s2_sqr);
    k      = sqrt(k_sqr);

    tmp1 = atan2(cy0, cx0);
    tmp2 = atan2(b, nx1 + a1);

    theta1_i  = tmp1 - tmp2;
    theta1_ii = tmp1 + tmp2 - M_PI;

    tmp1 = acos((s1_sqr+c2*c2-k_sqr) / (2*s1*c2));
    tmp2 = atan2(nx1, cz0-c1);
    tmp3 = acos((s2_sqr+c2*c2-k_sqr) / (2*s2*c2));
    tmp4 = atan2(nx1+2*a1, cz0-c1);

    theta2_i   = -tmp1 + tmp2;
    theta2_ii  =  tmp1 + tmp2;
    theta2_iii = -tmp3 - tmp4;
    theta2_iv  =  tmp3 - tmp4;

    tmp1       =  acos((s1_sqr-c2*c2-k_sqr) / (2*c2*k));
    tmp2       =  acos((s2_sqr-c2*c2-k_sqr) / (2*c2*k));
    tmp3       =  atan2(a2, c3);
    theta3_i   =  tmp1 - tmp3;
    theta3_ii  = -tmp1 - tmp3;
    theta3_iii =  tmp2 - tmp3;
    theta3_iv  = -tmp2 - tmp3;

    _s1[0] = _s1[1] = sin(theta1_i);
    _s1[2] = _s1[3] = sin(theta1_ii);

    _c1[0] = _c1[1] = cos(theta1_i);
    _c1[2] = _c1[3] = cos(theta1_ii);

    _s23[0] = sin(theta2_i   + theta3_i);
    _s23[1] = sin(theta2_ii  + theta3_ii);
    _s23[2] = sin(theta2_iii + theta3_iii);
    _s23[3] = sin(theta2_iv  + theta3_iv);

    _c23[0] = cos(theta2_i   + theta3_i);
    _c23[1] = cos(theta2_ii  + theta3_ii);
    _c23[2] = cos(theta2_iii + theta3_iii);
    _c23[3] = cos(theta2_iv  + theta3_iv);

    m_i   = e13*_s23[0]*_c1[0] + e23*_s23[0]*_s1[0] + e33*_c23[0];
    m_ii  = e13*_s23[1]*_c1[1] + e23*_s23[1]*_s1[1] + e33*_c23[1];
    m_iii = e13*_s23[2]*_c1[2] + e23*_s23[2]*_s1[2] + e33*_c23[2];
    m_iv  = e13*_s23[3]*_c1[3] + e23*_s23[3]*_s1[3] + e33*_c23[3];

    theta5_i   = atan2(sqrt(1 - m_i*m_i),     m_i);
    theta5_ii  = atan2(sqrt(1 - m_ii*m_ii),   m_ii);
    theta5_iii = atan2(sqrt(1 - m_iii*m_iii), m_iii);
    theta5_iv  = atan2(sqrt(1 - m_iv*m_iv),   m_iv);

    theta5_v    = -theta5_i;
    theta5_vi   = -theta5_ii;
    theta5_vii  = -theta5_iii;
    theta5_viii = -theta5_iv;

    if(fabs(theta5_i) < zero_threshold) {
        theta4_i = 0;
        theta6_i = opw_theta6_correction(theta1_i, rot);
    } else {
        tmp1 =  e23*_c1[0] - e13*_s1[0];
        tmp2 =  e13*_c23[0]*_c1[0] + e23*_c23[0]*_s1[0] - e33*_s23[0];
        tmp3 =  e12*_s23[0]*_c1[0] + e22*_s23[0]*_s1[0] + e32*_c23[0];
        tmp4 = -e11*_s23[0]*_c1[0] - e21*_s23[0]*_s1[0] - e31*_c23[0];
        theta4_i = atan2(tmp1, tmp2);
        theta6_i = atan2(tmp3, tmp4);
    }

    if(fabs(theta5_ii) < zero_threshold) {
        theta4_ii = 0;
        theta6_ii = opw_theta6_correction(theta1_i, rot);
    } else {
        tmp1 =  e23*_c1[1] - e13*_s1[1];
        tmp2 =  e13*_c23[1]*_c1[1] + e23*_c23[1]*_s1[1] - e33*_s23[1];
        tmp3 =  e12*_s23[1]*_c1[1] + e22*_s23[1]*_s1[1] + e32*_c23[1];
        tmp4 = -e11*_s23[1]*_c1[1] - e21*_s23[1]*_s1[1] - e31*_c23[1];
        theta4_ii = atan2(tmp1, tmp2);
        theta6_ii = atan2(tmp3, tmp4);
    }

    if(fabs(theta5_iii) < zero_threshold) {
        theta4_iii = 0;
        theta6_iii = opw_theta6_correction(theta1_ii, rot);
    } else {
        tmp1 =  e23*_c1[2] - e13*_s1[2];
        tmp2 =  e13*_c23[2]*_c1[2] + e23*_c23[2]*_s1[2] - e33*_s23[2];
        tmp3 =  e12*_s23[2]*_c1[2] + e22*_s23[2]*_s1[2] + e32*_c23[2];
        tmp4 = -e11*_s23[2]*_c1[2] - e21*_s23[2]*_s1[2] - e31*_c23[2];
        theta4_iii = atan2(tmp1, tmp2);
        theta6_iii = atan2(tmp3, tmp4);
    }

    if(fabs(theta5_iv) < zero_threshold) {
        theta4_iv = 0;
        theta6_iv = opw_theta6_correction(theta1_ii, rot);
    } else {
        tmp1 =  e23*_c1[3] - e13*_s1[3];
        tmp2 =  e13*_c23[3]*_c1[3] + e23*_c23[3]*_s1[3] - e33*_s23[3];
        tmp3 =  e12*_s23[3]*_c1[3] + e22*_s23[3]*_s1[3] + e32*_c23[3];
        tmp4 = -e11*_s23[3]*_c1[3] - e21*_s23[3]*_s1[3] - e31*_c23[3];
        theta4_iv = atan2(tmp1, tmp2);
        theta6_iv = atan2(tmp3, tmp4);
    }

    theta4_v    = theta4_i   + M_PI;
    theta4_vi   = theta4_ii  + M_PI;
    theta4_vii  = theta4_iii + M_PI;
    theta4_viii = theta4_iv  + M_PI;

    theta6_v    = theta6_i   - M_PI;
    theta6_vi   = theta6_ii  - M_PI;
    theta6_vii  = theta6_iii - M_PI;
    theta6_viii = theta6_iv  - M_PI;

    sols[6*0+0] = sols[6*1+0] = sols[6*4+0] = sols[6*5+0] = theta1_i;
    sols[6*0+1] = sols[6*4+1] = theta2_i;
    sols[6*0+2] = sols[6*4+2] = theta3_i;
    sols[6*0+3] = theta4_i;
    sols[6*0+4] = theta5_i;
    sols[6*0+5] = theta6_i;
    sols[6*1+1] = sols[6*5+1] = theta2_ii;
    sols[6*1+2] = sols[6*5+2] = theta3_ii;
    sols[6*1+3] = theta4_ii;
    sols[6*1+4] = theta5_ii;
    sols[6*1+5] = theta6_ii;
    sols[6*2+0] = sols[6*3+0] = theta1_ii;
    sols[6*2+1] = sols[6*6+1] = theta2_iii;
    sols[6*2+2] = sols[6*6+2] = theta3_iii;
    sols[6*2+3] = theta4_iii;
    sols[6*2+4] = theta5_iii;
    sols[6*2+5] = theta6_iii;
    sols[6*3+1] = sols[6*7+1] = theta2_iv;
    sols[6*3+2] = sols[6*7+2] = theta3_iv;
    sols[6*3+3] = theta4_iv;
    sols[6*3+4] = theta5_iv;
    sols[6*3+5] = theta6_iv;
    sols[6*4+3] = theta4_v;
    sols[6*4+4] = theta5_v;
    sols[6*4+5] = theta6_v;
    sols[6*5+3] = theta4_vi;
    sols[6*5+4] = theta5_vi;
    sols[6*5+5] = theta6_vi;
    sols[6*6+0] = theta1_ii;
    sols[6*7+0] = theta1_ii;
    sols[6*6+3] = theta4_vii;
    sols[6*6+4] = theta5_vii;
    sols[6*6+5] = theta6_vii;
    sols[6*7+3] = theta4_viii;
    sols[6*7+4] = theta5_viii;
    sols[6*7+5] = theta6_viii;

    return;
}

#undef c1
#undef c2
#undef c3
#undef c4
#undef a1
#undef a2
#undef b
#undef ux0
#undef uy0
#undef uz0
#undef e11
#undef e12
#undef e13
#undef e21
#undef e22
#undef e23
#undef e31
#undef e32
#undef e33