Kinematics and reverse kinematics lib for 6-7 DOF robot arms

Other Hardware Robotics
1

I am searching for a inverse kinematics lib for this kind of 6-7 DOF robot arms (target position e.g. = middle of the claw):

https://www.ebay.de/itm/Assembled-6DOF-Robot-Arm-Mechanical-Robotic-Clamp-Claw-with-Servos-Controller/252317775624?ssPageName=STRK%3AMEBIDX%3AIT&_trksid=p2060353.m1438.l2649

1600×1600 154 KB

As read from a different thread, probably matrix based versions are supposed to be most suitable.

Is there already such a lib existing, just to define the measures and dimensions of the legs and joints, then entering the required (x,y,z) target point, and then being able to calculate a practible solution for each joint which than could be applied to the servos (e.g. by analogWrite pwm) ?

(edit:)
also a straight kinematic lib would be fine to have, entering the dimensions and then all the single angles (alpha, beta, gamma, delta, epsilon,... for either joint ), and then to get the actual (x,y,z) position of the claw.

I personally don't have the maths skills to do that matrix math and also can't make Arduino math code out of matrix math symbols, but I think that that is already a very old and common problem which is already known for a long time to lots of users, so I actually think and hope that there will exist lots of practible, trusted and proven Aduino libs for that purpose.

2

This is what I have computed straight from Matlab using robotics toolbox and symbolic toolbox for your manipulator, just so you can see the problem with the implementation. The major issue is to make a library for 6DOF manipulators that is general in a sense that this example below only works for the manipulator I derived from DH convention of your manipulator, and still I cannot guarantee that it will work even then.

When you build or buy a robotic arm with 6DOF, you derive the kinematics for that arm specifically. The kinematics will differ from other 6DOF manipulators.

The homogenous transform matrix describing the orientation and position of the end effector based on the given joint angles q1 q2 q3 q4 q5 and q6 and link lengths (from "shoulder" to "elbow" to "wrist" to end effector) l1 l2 l3 and l4 is huge for a 6DOF manipulator, too big for the forum to allow me to paste it here (exceeds 9000 characters).

From the last column in the Homogenous Transform Matrix, you get the x, y, z position as:

x = l4*(cos(q4)*sin(q1) - sin(q4)*(cos(q1)*sin(q2 + pi/2)*sin(q3 - pi/2) - cos(q1)*cos(q2 + pi/2)*cos(q3 - pi/2))) - l3*(cos(q1)*cos(q2 + pi/2)*sin(q3 - pi/2) + cos(q1)*cos(q3 - pi/2)*sin(q2 + pi/2)) + l2*cos(q1)*cos(q2 + pi/2);
y = l2*cos(q2 + pi/2)*sin(q1) - l3*(cos(q2 + pi/2)*sin(q1)*sin(q3 - pi/2) + cos(q3 - pi/2)*sin(q1)*sin(q2 + pi/2)) - l4*(cos(q1)*cos(q4) - sin(q4)*(cos(q2 + pi/2)*cos(q3 - pi/2)*sin(q1) - sin(q1)*sin(q2 + pi/2)*sin(q3 - pi/2)));
z = l1 + l3*(cos(q2 + pi/2)*cos(q3 - pi/2) - sin(q2 + pi/2)*sin(q3 - pi/2)) + l2*sin(q2 + pi/2) + l4*sin(q4)*(cos(q2 + pi/2)*sin(q3 - pi/2) + cos(q3 - pi/2)*sin(q2 + pi/2));

Unfortunately, you will have to practice your maths skills if you want to implement anything like this, or wait until someone releases an appropriate library for this if you really want to do this in Arduino. There are solutions that can be used in Robotics Toolbox in Matlab that I would recommend. You can then run the kinematics calculations on your PC and communicate with your Arduino through Serial to move the joints.

This is the MATLAB code I used (Robotics toolbox and Symbolic toolbox required):

syms l1 l2 l3 l4 q1 q2 q3 q4 q5 q6 real
L(1)=Revolute('d', l1, 'a', 0, 'alpha', pi/2);
L(2)=Revolute('d', 0, 'a', l2, 'alpha', 0);
L(3)=Revolute('d', 0, 'a', 0, 'alpha', -pi/2);
L(4)=Revolute('d', l3, 'a', 0, 'alpha', pi/2);
L(5)=Revolute('d', 0, 'a', 0, 'alpha', pi/2);
L(6)=Revolute('d', l4, 'a', 0, 'alpha', 0);
AngleOffset=[0 pi/2 -pi/2 0 pi 0];
r=SerialLink(L,'name','6DOF Manipulator Arm','offset',AngleOffset);

T = r.fkine([q1 q2 q3 q4 q5 q6]);
disp(vpa(T,2))

I will look into this because of my own personal interest, maybe I will create a library for it using matrix math, we'll see. :wink:

1 Like
3

thank you for your answer!
Indeed all the calculations will have to be done autonomously, by arbitrary requirements and according to environmental conditions and according to detected objects to grab, lift, or lay down.
I actually thought that my ARM Cortex M4 feat. a fpu will be capable of this task - if it shouldn't I will have to move to a Raspberry Pi platform instead.
but tbh, I am really perplexed that I am the first one who is searching for such a thing, finally it's almost decades since robotic arms are on the hobby market, even for Arduinos.

To make the lib compatible to either 6DOF robot arm, I am quite sure that one just must get the chance to define the leg lenghts and axle types (e.g., enum ROTATE or TILT and perhaps LINEAR or LONGITUDINAL) for either leg and joint to make the lib work universally, e.g. by

// cusomize your arbitrary robot arm dimensions and design:

#define MAXJOINTS 6


Jtype[0] = ROTATE; // turntable joint
Jangl[0] = 270;  // max turn angle
Lleng[0] = 95;   // turntable height in mm

Jtype[1] = TILT; // shoulder joint
Jangl[1] = 180;  // max turn angle
Lleng[1] = 105;   // upper arm length in mm

Jtype[2] = TILT; // elbow joint
Jangl[2] = 180;  // max turn angle
Lleng[2] = 97;   // forarm length in mm

Jtype[3] = TILT;  // wrist tilt
Jangl[3] = 90;    // max turn angle
Lleng[3] = 65;  // wrist length: not exaclty linear mounted!

Jtype[4] = ROTATE;  // metacarpal rotate.
Jangl[4] = 90;       // max turn angle    
Lleng[4] = 50;    // metacarpal  length

Jtype[5] = LINEAR;  //  longitudinal finger length (not opening!!)
Jangl[5] = 0;     // virtual grab position, no motor used  (claw open/close external cmd)
Lleng[5] = 60;   // finger length = grab area length metacarpal to fingertip

I would appreciate very much if you might look at this!

4

When deriving the kinematics of a 6DOF manipulator, you have to assign coordinate systems to each joint so that they can be related to each other and to the origin in terms of orientation and position, depending on the design of your robotic manipulator, you will have to assign these coordinate systems differently. An approach that is used to assign these coordinate systems to simplify calculations is by using Denavit Hartenberg Convention: Denavit–Hartenberg parameters - Wikipedia.

I actually derived the DH parameters by hand for my previous post, it is not very hard for me since I study Robotics and I know the math.

Since the coorinate systems (frames) are assigned differently depending on the shape of your manipulator, the solution is not trivial. If you have one slight offset, for example, you will have very different formulas compared to if you wouldn't have this offset.

What would have to be done for such a library is to have an algorithm that assigns these DH parameters automatically based on the parameters the user inputs into it; For each joint i: Theta_i, D_i, Alpha_i, A_i. I think why this hasn't been implemented on Arduino yet is because it is a very difficult problem to generalize and Scientists aren't really interested in implementing things like this for Arduino. As I have said before, all this have been implemented in Matlab in Robotics Toolbox and it is very powerful. I find it very hard though to implement this in an Arduino library, but I guess it's not impossible, it just has to get a bit limited that's all. But note that not anyone can do this, it takes someone with knowledge within this area to implement a library for this.

5

yes, I see, thank you for your view to those details!
And looking to the bunch of matrices of the Denavit Hartenberg link I again realize why that will be off my limits.

About my joints and legs definitions: this was just meant as an interface to the user for easily passing the dimensions and measures to the matrices for just 2 simple basic kind of movements, i.e.
a) axis along the following leg (rotate) and
b) 90° vertically to the leg axis (tilt) ;
the computer then had to sort of "daisy-chain all that up" to get the actual single joints coordinates and pass all those values to the matrices, as you said.

I would highly appreciate if you took the efforts for developing such a simplified lib which is supposed to work on a 32bit Arduino core, e.g. the Due or the M4 (feat. a fpu, like by Adafruit or Teensy; I currently have both the Due, a M0 board, and the Adafruit Feather M4).
Of course I will support you by testing and debugging (and perhaps program API and user interface designing) as much as I can, if you will start working on this task. For a test platform, perhaps my robot arm with 6 180° servos (as shown above), attached by a I2C PCA9685 Servocontroller could be a good start, even as the design has got some severe drawbacks. If that once would work, then I would see if I'll find an even bigger and better one for further testing.
Perhaps even chargeable user licenses would be an option for you, for the later marketing.
OTOH, if the M4 turned out not to be powerful enough, we might move the project to the Raspberry Pi 2 or 3, featured by C++ and wiringPi.

6

The kinematics equations of the robot are used in robotics, computer games, and animation. The reverse process that computes the joint parameters that achieve a specified position of the end-effector is known as inverse kinematics. I hope this will help you a lot. If you still face any issue then check surfwindows for more information.

7

sapnasingh:
The kinematics equations of the robot are used in robotics, computer games, and animation. The reverse process that computes the joint parameters that achieve a specified position of the end-effector is known as inverse kinematics. I hope this will help you a lot. If you still face any issue then check surfwindows for more information.

hahaha, you think that helped me a lot ? "rofl*
That was actually my TOP question for a matching lib ! :stuck_out_tongue:

1 Like
8

dsyleixa:
yes, I see, thank you for your view to those details!
And looking to the bunch of matrices of the Denavit Hartenberg link I again realize why that will be off my limits.

About my joints and legs definitions: this was just meant as an interface to the user for easily passing the dimensions and measures to the matrices for just 2 simple basic kind of movements, i.e.
a) axis along the following leg (rotate) and
b) 90° vertically to the leg axis (tilt) ;
the computer then had to sort of "daisy-chain all that up" to get the actual single joints coordinates and pass all those values to the matrices, as you said.

I would highly appreciate if you took the efforts for developing such a simplified lib which is supposed to work on a 32bit Arduino core, e.g. the Due or the M4 (feat. a fpu, like by Adafruit or Teensy; I currently have both the Due, a M0 board, and the Adafruit Feather M4).
Of course I will support you by testing and debugging (and perhaps program API and user interface designing) as much as I can, if you will start working on this task. For a test platform, perhaps my robot arm with 6 180° servos (as shown above), attached by a I2C PCA9685 Servocontroller could be a good start, even as the design has got some severe drawbacks. If that once would work, then I would see if I'll find an even bigger and better one for further testing.
Perhaps even chargeable user licenses would be an option for you, for the later marketing.
OTOH, if the M4 turned out not to be powerful enough, we might move the project to the Raspberry Pi 2 or 3, featured by C++ and wiringPi.

I have begun working on a library for this. It is currently uncommented and unusable since the inverse kinematics is not quite done yet. Forward kinematics should work, though. The plan will be to make this library able to compute FK and IK for most manipulators, we'll see how far I will be able to take this. I will update the repository as I develop the library so you can keep up to date.

9

henrik, that is really awesome! I'll follow that link and it's progession and gladly looking forward to it! Thanks a lot that you'll take the efforts of doing that! 8)

10

@henrik,
by intermediate experiments I meanwhile came upon another problem about the claw:
how would you define, if an object has to be gripped by the claw's finger tips (e.g., a chess piece) or by the "full hand", e.g. to grab a bottle?
Addionally, the yaw, pitch+roll orientation of the claw should be possible to be defined for FK and IK, e.g. to pick a chess piece at the top from above, a bottle horizontally from either side, or a slanted object from a diagonal position.

as to float vs. double, as stated before, single-float is surely precise enough for FK; but I was meanwhile recalling issues about floats in matrix determinats which leaded to false values (falsely positive instead of actually zero in some cases) which then leaded to false, invalid inverted matrices, even with nans.
So as to det and Inverted matrices to IK, double fp is probably indispensible.

11

Determining gripping offset is easy using Inverse Orientation Kinematics, I am currently looking into it. I have gotten inverse kinematics working for position, but I am having trouble getting the orientation to converge. I am using inverse velocity kinematics to solve IK iteratively. I am currently working on a solution with Kinematic Decoupling. You will thus be able to choose a gripping offset with one value. I could actually implement this feature in my other library "Fabrik2DArduino", which takes advantage of Kinematic Decoupling to solve for the tool angle.

I will change everything to "double" to see if there is any major improvement in performance.

12

just observed after some basic experiments with my M4 and the pca9685 Servocontroller, that the 5 or 6 DOF arm above is very limited by it's working space and for objects handling.
A 7-DOF like this one will much more versatile:

800×800 46 KB

https://www.ebay.de/itm/Arm-7-Axis-Robot-Arm-7DOF-Arm-High-Torque-Servo-For-DIY-Education-Mechanical/152943121372?ssPageName=STRK%3AMEBIDX%3AIT&_trksid=p2060353.m1438.l2649

The biggest drawback is always the servo angle to mostly just 180°, so 1-2 extra joints ( i.e., DOFs to the FK/IK matrices) had to be added.

13

That looks similar to the robotic arm from here:

Maybe you should try his code? It is made for his particular arm so it may not work.

14

hello Henrik,
can you please describe in your lib examples 6DOFLib/src.ino at master · henriksod/6DOFLib · GitHub how the user has to enter
a) the length of all legs (turntable height, upper arm, forearm1, forearm2, wrist1, wrist2, finger length)
b) the geometrical orientation of all joints (turntable=rotate, shoulder=tilt, elbow=tilt, middleforearm=rotate, wrist1=tilt, wrist2=rotate) ,
c) the maximum joint angles (e.g., 180°, 270°, 360°, infinite), and
d) the straight zero position of all joints (e.g. for a 270° servo: 135° for the center of the working space vs. 90° by a 45° offset)
?
(in the middle of the forearm there is now an extra rotational joint roughly as shown in the last picture)

15

Hello, my internet has been down for two weeks and I have had a lot of studying to do. To answer your question, I will refer to a guide for assigning DH parameters, I will include an example procedure in the library description when I am done. I will also move the library to another repository that is more official.

What is left to do is:

  • Comment code
  • Improve efficiency of numerical IK (Will take reference from Peter Corke's Robotics Toolbox)
  • Make a more user-friendly interface
  • Create library description for repo
  • Test for various robotic manipulators, make sure it is consistent

Denavit-Hartenberg Reference Frame Layout:

16

thanks for your update!
As to Denavit-Hardenberg, this is still too complicated for me to understand with all it's axis and arms transformations. Perhaps some examples for standard robotarms (e.g., like posted by me above) would make it more easy to find into all those details.
About github, I actually think that this is an almost perfect place for hosting, download, and especially to open and discuss issues; tbh, I would appreciate very much if it wioll be still there in the future. But perhaps for explanations, diagrams, charts, pictures, and tutorials an additaional website will have some benefits.
Thank you very much for your efforts and I wish you great success!

17

It will still be on GitHub, but maybe with a different name. I think that you should start with a much simpler robotic arm and use my other library to get a hang of what is going on. I think going straight for 6DOF manipulators is a bit too ambitious given that you don't know how to perceive the concept of Denavit-Hardenberg. My other library is tailored for people with no knowledge in the field, so they can get to controlling their robotic arm right away. Who knows, maybe you can still use it for a 6DOF manipulator with some modifications or smart use of the library?

I will continue with working on this library, but I cannot guarantee it will be near as fast as other libraries out there.

I have updated my other library with possibilities for a rotating base and adjustment of gripper offset.

18

as to 6DOF vs simpler robot arms I have to disagree - IMO it's a matter of the user interface to make it useable for common Arduino users. e.g., also the Arduino Braccio robot arm provides 6DOF.
As sated, some examples for standard robotarms (e.g., like shown by me above) would make it more easy to enter all correct settings and details for either proprietary robot arm.

Once having entered measures and dimensions of arm lengths and angle orientations by a simplified, user-friendly pattern, then the library would make no difference for the end user, no matter how many DOFs it has, both for FK and for RK, because finally one would need just
a) for FK: enter the actual single actual angle values (3...7 angles depending on the number of provided DOFs)
b) for RK: enter the 3D target point for the claw position (claw base position plus offset) and the claw vector orientation (yaw, pitch, roll).

19

henriksod:
It will still be on GitHub, but maybe with a different name.

Just having returned from vacation, I took a look at your 6DOF example

tbh, I have to admit that I can't see a way to enter the actual arm segment lengths of all legs:

#include "SixDOF.h"
#include "MatrixMath.h"
#include "MemoryFree.h"

double thetas[] = {0, PI/2, -PI/2, 0, PI, 0};
double ds[] = {0.25, 0, 0, 0.2, 0, 0.2};
double alphas[] = {PI/2, 0, -PI/2, PI/2, PI/2, 0};
double as[] = {0, 0.25, 0, 0, 0, 0};

double jointAngles[] = {0, PI/3, PI/2, 0, -PI/4, 0};
double jointAngles2[] = {0, PI/4, 0, 0, PI/3, 0};

double pose[] = {0, 0, 0, 0, 0, 0};

double desiredPose1[] = {-0.4501, 0.15, 0.5777, 2.2923, 1.5234, -0.6599};
double desiredPose2[] = {-0.4485, 0.1225, 0.1147, 2.3934, 2.0215, 0.5019};
double desiredPose3[] = {0, -0.45, 0.05, -1.5708, 1.5708, -3.1416};

SixDOF manipulator(thetas, ds, alphas, as, 6);
20

how to use the example program for a proprietary robot arm?

e.g, this is my current design, just to show what I am talking about:

\   /
 \ /      finger length       50 mm
 |-|  6   claw spread   
  |       metacarpal length   55 mm
  |       
  o   5   metacarpal rotate
  |     
  |       wrist length        95 mm
  |     
  v   4   wrist tilt       
  |       
  |       forearm segment2    50 mm
  |       
  o   3   forearm rotate
  |
  |       forearm segment1   100 mm
  |
  v   2   elbow tilt     
  |       
  |
  |       upper arm length   180 mm
  |
  |         
  v   1   shoulder tilt
  |
  |       shoulder height     15 mm
  o   0   trunc rotate horiz   
  |      
  |       trunc base          85 mm
  |
 ___      floor