Ikine 4 DOF arm

Lynxmotion Robot Arms
21

Hi kalimeras61 and also other interested people.

I will explain how the ikine works with one example.
All the angles are in degrees !
My robotconfig has these values (in mm):
H = 165
E = 155
F = 155
G = 85

The servo’s of this robot has following ranges:
servo: min and max in degrees

sv1: -90 and 90
sv2: 0 and 135
sv3: -150 and 0
sv4: -90 and 90

The target position is the following:
px= 65,6393583641561
py= 5
pz= 165,785186268066

And thegripper makes 164,6 degrees with the positive Z axis !
Z_angle= 164,6

Question: find values theta1,theta2,theta3 and theta4

Solution:

With px and py you can find theta1:
theta1 or phixy= 4,35602560258367

Now you have to find the wristposition pxw,pyw,pzw:

pxw= 43,1322919852168
pyw= 3,28555100629763
pzw= 247,733295627966

This wristposition must be 85 mm from the target position px,py,pz.
If you check this, you see that: linkdist= 85

Now we have to find the first three angles: theta1, theta2 and theta3

We already know theta1. theta1 is 4,356 or -175,644 degrees

Next we calculate theta3:
theta3 is 144,945 or -144,945

Now we have to calculate theta2. Theta2 has to be calculated in respect of theta1 and theta3. So we have 8 combinations:
column1=theta1,column2=theta3,column3=theta3
column4=theta4 is zero for now. This will be filled in later

----- first 3 angle solutions---------
4,356 -10,076 144,945 0,000
4,356 -10,076 144,945 0,000
4,356 134,870 -144,945 0,000
4,356 134,870 -144,945 0,000
-175,644 -169,924 -144,945 0,000
-175,644 -169,924 -144,945 0,000
-175,644 45,130 144,945 0,000
-175,644 45,130 144,945 0,000

In this matrix every uneven row is the same. But this is for later.

Now that we know theta1,theta2 and theta3, we can calculate theta4 for each row:
----- ALL angle solutions---------
4,356 -10,076 144,945 150,530
4,356 -10,076 144,945 -150,530
4,356 134,870 -144,945 64,524
4,356 134,870 -144,945 -64,524
-175,644 -169,924 -144,945 150,530
-175,644 -169,924 -144,945 -150,530
-175,644 45,130 144,945 64,524
-175,644 45,130 144,945 -64,524

This matrix gives all possible robotconfigurations. The 8 rows are only in the case of a 6 DOF robot. But here we have only 4 DOF.
So now we have find the rows from where the forward transformation is close to the target point. If the this distance is smaller than 0,1 mm then
we have a correct POSSIBLE robotconfiguration.

When we apply this method we become next situation:
----- Calculated results ---------
4,356 -10,076 144,945 150,530
4,356 134,870 -144,945 -64,524
-175,644 -169,924 -144,945 -150,530
-175,644 45,130 144,945 64,524

The 4 rows that we become here are the 4 real possible configurations of the robot. But in practice the servo’s that are used have their ranges (see above).
This means that only a few solutions (or 1 solution) will be the PRACTICAL solution !!

So when we check these angles with the servo ranges we see that only row 2 is the exact solution:

4,356 134,870 -144,945 -64,524

In the case that there are two (or more) solutions. My program takes always the first chosen row.

22

dear nowis
while claculation theta4, it always gives an imaginer result, because the expression in the squareroot in formulation we use for calculating theta4 is always lower than zero. So, taking the squareroot of a negative value is a complex number! how did you handle this in your coding?

cos(theta4)=ksi=(C23*(C1px+S1py-eC2)+S23(pz-h-e*S2)-f)/d;

theta4=atan2(±sqrt(1-ksi^2)/ksi)

in this expression the value of ksi is always negative, so the value of theta4 is always a complex number. can you give some advise for handling this comdition?
I tried to use the initial values you gave in your previous post. but again, I got the same imaginer result. So I’m gonna need your guideness again if you don’t mind.
thanks a lot for the help by now.

23

Hi kalimeras61,

I think the problem lies in the calculation of the angle.
I had the same experience.

The angle is mathematically calculated by the Math.Atn() function (in VB6).
Well this function just calculates the value that is given…

You must consider the coordinates of the point (px,py).
When px is positive there is no problem.
But when px is negative, the result of the Math.Atn() is wrong by 180 degrees !

Here is an example:
px= 6700.86807
py= 8065.41592
–> calculated angle by Math.Atn() = 50.2797 : correct

px= -6700.86807
py= 8065.41592
–> calculated angle by Math.Atn() = -50.2797 : is wrong because that point lies in quadrant(x>0,y<0) !
The correct answer lies 180 degrees back !
angle= -50.2797 + 180 = 129.7203

px= -10267.85511
py= -2126.90342
–> calculated angle by Math.Atn() = 11.70285 : is wrong because that point lies in quadrant(x>0,y>0) !
The correct answer lies 180 degrees back !
angle= 11.70285 - 180 = -168.297

To overcome this problem I created my own Argtan(py,px) function:

Function Argtan(py As Double, px As Double) As Double
’ calculation of the angle based on py and px

Dim division As Double
Dim Angle As Double
If px = 0 Then
    'MsgBox "Argtan: division by zero", vbCritical
    Angle = 90
    GoTo JMP
End If
division = py / px

Angle = Math.Atn(division) * 180 / PI 'conversion to degrees !!
If py = 0 Then Angle = 0
If (px < 0 And py > 0) Then Angle = Angle + 180
If (px < 0 And py < 0) Then Angle = Angle - 180

JMP: Argtan = Angle
End Function

Hope this helps you out.

24

dear nowis
thank you very much for your kind interest on my questions. your explanations did help and I’ve recently been able to run your codes on MATLAB.

The sample values you gave above while explaining the combinations of arm angles seem to be calculated with a formula before because they all are fractional. you also gave an sample value for “alfaz” , the orientation angle. the value you gave is 164.6 and I still wonder how it is calculated and what does it exactly means! this angle shows the orientations of the last arm with respect the the hand on the z axis or something like that? I wonder if u could help me understand how it is calculated and what’s its physical meanning on the configuration. thanks for the help by now…

25

Hi kalimeras61,

At last an easy question :wink:
I am happy that you solved all the parts of the puzzle :smiley:

Alfa Z is not calculated at all. It is given !
I just took Z_angle= 164,6 because my robot was orientated like that.

The Z Angle is the angle that the gripper makes with the positive Z-axis.
In this 4 DOF robot there is only one axis that you can rotate too in a point (px,py,pz).
You cannot rotate around the X- or Y-axis because these are fixed by the position px and py.

In the youtube movie, where the square is drawn, the Z_angle is 90 degrees at all times.
When Z_angle= 0 the gripper points in the same direction as the Z-axis.
When Z_angle= 180 the gripper points in the opposite direction as the Z-axis.

I hope everything is clear now 8)

26

and that means the hand position relative to the ground is always the same? that’s what I was afraid of… because I was expecting it to claculate this angle itself too but I guess I suck in this manner again. I’ve been looking for an article or a study including this arm moving too but couldn’t find one. have u ever found such a study including the self-movement of this hand part?

27

Hi kalimeras61,

I don’t quite understand your question.
The beauty of the inverse kinematics is that you can control your gripper
in a 3D wordl with orientation and without calculating every angle of the links.
Here you have only one orientation around the Z-axis that you can choose.

But all the intelligence is in the forward and inverse kinematics.
As a user you just position your gripper how you want it and the
program does all the rest.

The angle of the gripper relative to the ground is calculated in the forward transformation when you move your arm.

Greetings
nowis

28

dear nowis
hello again, after all this long time. how are you doing? Last year, after you sent me your visual basic code of your arm, I transferred the codes to MATLAB environment and could finally make it run. But it seems like the angle value of the 4th arm (last arm) is always constant because it is given before the IK calculations are processed. haven’t you been able to find a way so that the arm can claculate all o f the 4 angles including the position of the last arm? otherwise it feels like a 3 dof robot arm instead of a 4dof because we are determining one of the angles, not the arm itself. I wonder if you could share any solution you might have extracted to make the 4th arm’s position be calculated by itself, not given by the user. Maybe we can also extract a relation between some of the angles to make all the angles be calculated automatically without requiring a previous positioning. thank you very much for your kind concern again.

29

Hi kalimeras61,

I found some time to reply.
I think you miss the principle of the calculations.
With a 3 DOF arm you can reach any point in space. But not with a desired orientation (X_angle,Y_angle,Z_angle).

With a 4 DOF am you can also reach any point in space. But with only one desired orientation.
In this configuration of the Lynxmotion arm is so that the orientation of the Z angle can be chosen.
So in one point in space (px,py,pz) you can choose the orientation of the TCP relative to the Z axis (Z_angle).

In the previous posts I gave one mathematical example.
There was given:
px= 65.6
py= 5
pz= 165.7
Z_angle= 164.6

The question was to find the joint angles theta1,theta2,theta3 and theta4.
The final solution was: theta1=4.356, theta2=134.870, theta3=-144.945, theta4=-64.524

So as you see here theta4 is totally different than the Z_angle !
Each joint angles is always relative to the previous coordinate frame.
In my “drawing square” movie the Z_angle is always constant, because the pen always has to be vertical
to the paper in the world coordinate frame. But theta4 is changing constantly in the relative coordinate frame.

If your calculations are correct you should be able to find the joint angles for the same point
in space (px= 65.6,py= 5,pz= 165.7) but with a Z_angle of 150 degrees (for example).
The joint angles will become different. Just theta1 will be the same here.

Let me know if you understand this.

Greetings,
nowis

30

Interesting comments.

Would you be meaning “With a 4 DOF am you can also reach any point in space. But with ANY desired orientation”?

I see this for arms. I’m working with a 4DOF leg. I hadn’t thought of wanting a specific direction to the foot (a ball tip), but certainly if a true foot (flat to the ground) was added. Do you see any corollary here?

Alan KM6VV

31

Hi,

I stated “With a 4 DOF am you can also reach any point in space. But with only **ONE **desired orientation.”
If you want **ANY **orientation in a point in space you will need a **6 DOF **robot with 3 a DOF wrist.
This lynxmotion arm has only 1 DOF wrist…

But I think in your case this is not necessary. A 4 DOF leg will do the job for walking.
I see the following corrolation:
arm robot: world coordinate frame has XY axis in a horizontal plane. The Z axis points in the sky.
leg robot: world coordinate frame has XY axis in a horizontal plane. The Z axis points into the ground !

The only difference is that in the leg robot the world coordinate frame is rotated around the X axis for 180 degrees.
So for a leg the Z axis is always pointed to the ground.
If the tip of the leg has to be vertical to the ground then Z_angle= 0
If the tip of the leg has to be parallell to the ground then Z_angle= 90

If I would do this the Z_angle would always be zero. This becomes an offsetvalue if you like.
And then you only have to focus of the startpoint (px1,py1,pz1) and the endpoint (px2,py2,pz2) .
The tricky part here is that, when a path is followed, the world coordinate frame moves and the tip of the leg stays stationary ! It is working in the opposite way. You must be fully aware of the directions of the world coordinate frame for each leg.

Hope this helps you out.

32

Thanks for the explanation! It will take a few readings to totally sink in. 6DOF might be a bit much for simple arms.

This orientation problem also applies to wheeled vehicles. Just getting from A to B is not all the navigation one needs. The 'bot in many cases must also face the proper direction.

Much to consider!

Thanks!

Alan KM6VV

33

hi all, how can i found pxw,pyw,pzw anyone knows that? what is alpha(z) in pdf?

34

Hi all,

First of all sorry whether what I am saying it is completely wrong, but I am really curious about how the tranformation matrices for each joint where calculated.

I was reading the entire PDF about the 4 DOF because I need something similar to perform the kinematics of one of my robots. I completely agree with the way the DH parameters are obtained however I strongly desagree with the way the T matrix is calculated. Did you used any initial resting position? I mean theta1, theta2…theta4 are given any initial value? If so please skip the whole post because I am wrong, otherway I should assume that these angles are completely unknown.

The transformation matrix for two joints in parallel (i and i-1) is given by a 4x4 matrix where the displacement bit of the matrix (row) is [aicos(theta1) aisin(theta1) di 0]T. However looking at your transformation matrices and specially focusing on the T23 one where d = 0, theta = theta3, a = e and alpha = 0 your displacement is represented by [e 0 0 1]T rather than by [ecos(theta3) esin(theta3) 0 0]T . How did you get to this poing omiting the thetha angle? Am I missing anything?

I have checked the book you suggest and some other additional books and I really cannot find how you get that… . As I aforesaid whether I am wrong could you shed me some light into the spot?

Thanks :slight_smile:

35

Hi Luis,

Sorry for the late reply but I can guarantee that my calculations are correct.
This because inverse equations are derived from the these DH matrices. If there would be
one mistake then the ikine would not work at all.
For the T23 I will explain later.

Greetings

36

Hey nowis
I’m have written my own IK-code to control a 4 DOF arm, and it works, but my code doesn’t consider multiple solutions and min/max joint angles. I have looked through your pdf, but the handwriting confuse my a bit - though I understand most of it. Would it be possible to get your code on email, to check out how you did it, and use it as inspiration to make my own code? - I would really appreciate it.
Thanks

/Rasmus

37

Hi

I had a look at the inverse kinematic pdf. It was very uselful. What is alpha z that he refers too? “orientation z angle” he says on page 14 line 5.

Any help would be great

Thanks

38

Please ignore my comment.

39

Hello. I realize this is an older thread, but I’m hoping nowis might still be here, or someone else familiar with kinematics to verify something in his math.

I’m teaching myself kinematics and am comfortable with the forward kinematics, transformation matrices etc. Since I have an al5d arm, I am going through nowis’ math as an educational and practical exercise in finding the equations for ik. While I’m doing this I am moving nowis’ work to a Latex document so it will be more readable/accessible for others (with all credit given to nowis).

On the top of page 3 is the matrix multiplication to get the transformation matrix to go from frame 4 to frame 0. Check out the the 1st column 3rd row entry
for the result. It shows

sin(theta2 + theta3)cos(theta4) + cos(theta2 + theta3)cos(theta4)

I think it should be

sin(theta2 + theta3)cos(theta4) + cos(theta2 + theta3)sin(theta4)

I found this by doing the forward kinematics with the robot at rest/home position (all angles = 0).
The link length d (wrist to gripper) shows up in both the x and z coordinates.

My dilemma is that after watching nowis’ robot draw a square, his ik works.

Thanks for any insight,

Joel

40

Hi Joel,

I just got a notification on my mail and read your post.
I am very happy that someone thoroughly read my calculations.

The message that you gave about my calculation is correct. I made a mistake on page 3.
As you see I am also human :slight_smile:

The right calculation should be as you mention: sin(theta2 + theta3)cos(theta4) + cos(theta2 + theta3)sin(theta4)
Then this can be simplified to: sin(theta2 + theta3 + theta4) or s234.

So on page 3 there must be s234 instead of c4(s23+c23) for the third row and first column element.
On page 4 for the resultmatrix of the forward transformations there are two elements that needs to be changed:

  • row 3 column 1: s234
  • row 3 column 4: ds234+fs23+e*s2+h

Luckily the calculations (inverse kinematics) from page 5 and further are correct.

Greetings and thanks for pointing this out,
Nowis