Go Down

Topic: Kinematics and inverse kinematics lib for 6-7 DOF robot arms  (Read 826 times) previous topic - next topic

dsyleixa

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



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.


henriksod

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:

Code: [Select]

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):
Code: [Select]
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. ;)

dsyleixa

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

Code: [Select]
// 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!

henriksod

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: https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters.

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.

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.

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.


dsyleixa

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 ! :P

henriksod

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.

https://github.com/henriksod/6DOFLib

dsyleixa

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)

dsyleixa

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

henriksod

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.

dsyleixa

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.


henriksod

That looks similar to the robotic arm from here:
https://www.youtube.com/watch?v=AGHwW5TSNCY

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

dsyleixa

hello Henrik,
can you please describe in your lib examples https://github.com/henriksod/6DOFLib/blob/master/src/6dof/src.ino 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)

Go Up