Inverse kinematics for 4-DOF Articualted manipluator

Robotics Asked by Oualid on October 3, 2021

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: %fn", theta1);
   printf("A: %fn", A);
   printf("B: %fn", B);
   printf("C: %fn", C);

   printf("theta3: %fn", 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

Answered by Oualid on October 3, 2021

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.

Answered by Ben on October 3, 2021

Add your own answers!

Related Questions

What does the normalized image coordinates imply?

0  Asked on October 3, 2021 by lzx071021


Rapid code pick and place

1  Asked on October 3, 2021 by ramil-aliyev


How can I get the equations of motion used in webots?

1  Asked on October 3, 2021 by q-than-a


Handling Inverse Kinematics error on a UR Robot

1  Asked on October 3, 2021 by user24261


ROS2 and TypeError when publishing custom message to Topic

1  Asked on October 3, 2021 by anthares


BLDC Stuttering when attached to Arduino Nano

1  Asked on October 3, 2021 by moazzam-salman


Determining required torque for DC motor

0  Asked on October 3, 2021 by timetraveller


Ask a Question

Get help from others!

© 2023 All rights reserved. Sites we Love: PCI Database, UKBizDB, Menu Kuliner, Sharing RPP