I am trying to find out the math formulas for Inverse Kinematics for a 4-DOF articulated manipulator.

4-DOF articulated manipulator diagram

By Googling I found information about inverse kinematics for 3-DOF, 4-DOF and 6-DOF articulated manipulator, but very few information for 4-DOF robot arm.

But what I found is a paper explaining how to calculate FK and IK for a 4-DOF robot arm:

Kinematics Modeling of a 4-DOF Robotic Arm

I tried to implement the math formula in C code but the calculation doesn't seem to work.

I believe that the formulas are wrong.

The math formula algebraically is the as follow:

enter image description here

dx, dy and dz are the global en effector coordinates.

Angle phi is the end effector orientation.

The C-code looks like this:

#include <stdio.h>
#include <math.h>

#define M_PI   3.14159265358979323846264338327950288

#define deg2Rad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)
#define rad2Deg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)

int main()

    float l1, l2, l3, l4;   // Links mm
    float dx, dy, dz;       // EE position
    float phi;              // EE orientation
    float theta1, theta2, theta3, theta4;   // Joint variables that will be calculated
    float A, B, C;
    l1 = 170.0;
    l2 = 45.0;
    l3 = 85.0;
    l4 = 130.0;
    phi = 0.4;

    dx = 30.0;
    dy = 15.0;
    dz = 20.0;

    theta1 = atan(dy/dx);

    A = (dx - l4 * cos(theta1) * cos(phi));
    B = (dy - l4 * sin(theta1) * cos(phi));
    C = (dz - l1 - l4 * sin(phi));

   theta3 = acos(((A*A+B*B+C*C)-(l2*l2)-(l3*l3))/(2*l2*l3));

   printf("theta1: %f\n", theta1);
   printf("A: %f\n", A);
   printf("B: %f\n", B);
   printf("C: %f\n", C);

   printf("theta3: %f\n", theta3);

   return 0;

When I calculate theta3 I get nand because what is inside acos is greater than 1 and acos can't have a value greater than 1. The conclusion is that something is wrong with the formulas.

Any suggestion?


If your image is correct, it appears that you have a "planar 3 link arm" mounted on a turn-table. If so, I would suggest that you decompose the problem into yaw (joint 1), and X/Y in the plane (joints 2-4). It will probably be easiest to use polar coordinates.

You should be able to find many solutions online for a planar 3 link arm.

  • $\begingroup$ But what about the rotation angle q1 around z axis for the base, it doesn't count? We have four angles to calculate: q1,q2,q3 and q4 (theta in the code). If I understand correctly the robot arm in the picture has 4 links: link1=base, link2=shoulder, link3 = elbow, link4=wrist. $\endgroup$ – Oualid Jul 7 '20 at 21:22
  • $\begingroup$ I suppose that the robot arm is not a 4 DOF but a 3 DOF robot arm. $\endgroup$ – Oualid Jul 7 '20 at 21:50

I found what I was looking for:



Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.