# Inverse kinematics for 4-DOF Articualted manipluator

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

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:

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?

## 2 Answers

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.

• 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. Jul 7 '20 at 21:22
• I suppose that the robot arm is not a 4 DOF but a 3 DOF robot arm. Jul 7 '20 at 21:50

I found what I was looking for:

https://www.robotshop.com/media/files/PDF/robotshop-multi-purpose-robotic-arm-guide.pdf

• Did you find the Inverse kinematics of this paper? Jun 16 at 16:23