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

@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.

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.

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:

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.

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.

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)

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:

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!

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.

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).

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);

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

I am trying to decide whether to use this or a modification of the "genserkins" module from LinuxCNC.
(The latter has the advantage of being more familiar, but includes some rather "excessive" libraries.)

I am wanting to use forward kins only, to determine the position of the end of a Faro arm. (Which I found cheap on eBay).

andypugh:
I am trying to decide whether to use this or a modification of the "genserkins" module from LinuxCNC.
(The latter has the advantage of being more familiar, but includes some rather "excessive" libraries.)

I am wanting to use forward kins only, to determine the position of the end of a Faro arm. (Which I found cheap on eBay).

for my purposes, both FK and RK actually are crucial.

any updates about a adjustable FK+RK lib for 6-7 DOF robotic arms?
AFAIK neither lib of the ones mentioned above would work so far actually for proprietary arm constructions (CMIIW)...?

Hi,

haven been trough this, years ago.
Have done the forward and reverse KIN, from zero.

Reverse KIN is the difficult part.
Unfortunately, I don't have a LIB or something for you.

If your accuracy is not high, you could make a multidimensional ARRAY.
( is a lot of work in case of 6 DOF )

ARRAY [x,y,z, u,v,w] as input and [a, b, c, d, e, f] as output, the 6 angles of the servo's .

x,y,z are the coordinates of the gripper(tip) and u,v,w the orientation.

But this is allmost undoable...

Kind regards.

well, the required precision should be close to 2mm.
The workspace of the arm is a 3/4 sphere by a radius of a about 500mm, that's
4pi3/4 = 3pi*r³ ≈ 1,250,000,000mm³ .
CMIIW, but that would require ≈100,000,000 (x,y,z) coordinates plus relates joint angles,
plus all additional gripper orientations -
and finally:
I cannot even determine the joint angles because the servos don't return angle positions. :stuck_out_tongue:
So unfortunately, I'm afraid, your proposal about look-up tables is really not doable... :wink:

Nonetheless, mathematically it should be solvable though (by theory), using inverse and transposed matrices from FK (but that is far beyond my skills 8) ) .

Sorry, i can't help you further.

I aproached my FW and BW in my own way!
( at the end, it looks like a kinda array/matrix-system, too )

Good luck and succes with the progress!

anyone who knows how to control a 6-7DOF robot arm like the following by Reverse Kinematics?
Just define the spatial coordinates to move the gripper to, and the lib is supposed to calculate either required servo angle?
And the same (easier task) vice versa for Forward Kinematics?
I still can't find a working lib for that purpose... :frowning:

\   /
 \ /      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

Look at the AR2 robot arm software.

It is all open source. You can set the segment lengths in the python interface, which does all of the inverse kinematics for a 6 axis robot arm. You will have to re-write the Arduino portion to control the servo position, as it is written for a stepper motor instead of a servo.

Did you ever solve this problem?