0
$\begingroup$

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?

Improve this question
$\endgroup$

2 Answers 2

Reset to default
1
$\begingroup$

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.

Improve this answer
$\endgroup$
2
  • $\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, 2020 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, 2020 at 21:50
0
$\begingroup$

I found what I was looking for:

RobotShop M100RAK Modular Robotic Arm Kit

Improve this answer
$\endgroup$
2
  • $\begingroup$ Did you find the Inverse kinematics of this paper? $\endgroup$
    – ooo_e
    Jun 16, 2021 at 16:23
  • $\begingroup$ Please remember to mark your answer as the accepted answer $\endgroup$
    – Greenonline
    Oct 19 at 9:45

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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