# What is my last rotation matrix for the last three angles when i have found the first three when doing inverse kinematics to a 6dof robot?

I am doing inverse kinematics for a 6dof robot.

I have used the geometrical method to describe the angles from one to three. I have trouble doing the next part. I know I that I have found the location by trigonometry* and now I need to find the rotation algebraically. Since it is the rotation I can use the rotation matrix that describes the rotation from 0 to 6. Since I know my angles from 0 to 3 I can do this:

$$T_6^0=T_0^3\cdot T_3^6$$

You can take the inverse of $$T_0^3$$ on both sides and you end up with

$$(T_3^0)^{-1} \cdot T_6^0= T_3^6$$

I don't know what to do now. I know I have to equate the different terms but what terms. How do I get the rotation matrices to solve the inverse kinematics? Essentially my question is how do I get the rotation matrices that will let me solve the problem. How do I insert angle $$\theta_1$$ to $$\theta_3$$?

*Just a side question: They say that in 6d0f freedom robot the first three give the location and the last three the rotation. However if I change $$\theta_5$$ on a 6dof the location changes too? How come? The only one who doesn't change the location is $$\theta_6$$.

Here is my matlab code. I have also added a picture of the lengths and the DH-table we made. https://i.imgur.com/6QTCO2P.png [![enter image description here][1]][1]

The desired angles are 10,20,30,40,50,60 for theta1 to theta 6.

    %% Definitions.
clear all
syms theta1 theta2 theta3 theta4 theta5 theta6

%% MatriX
T01=[ cos(theta1), -sin(theta1), 0, 0;
sin(theta1),  cos(theta1), 0,   0;
0,                   0, 1,   0;
0,                   0, 0,   1];

T12= [cos(theta2) -sin(theta2) 0 25;
0                0     -1 0;
sin(theta2) cos(theta2)  0 0;
0              0        0 1];

T23=[ cos(theta3 + pi/2), -sin(theta3 + pi/2), 0, 315;
sin(theta3 + pi/2),  cos(theta3 + pi/2), 0,   0;
0,                   0, 1,   0;
0,                   0, 0,   1];

T34= [cos(theta4) -sin(theta4) 0 -35;
0                0     -1 -365;
-sin(theta4) -cos(theta4)  0 0;
0              0           0 1];

T45= [cos(theta5) -sin(theta5) 0 0;
0                0     -1 0;
-sin(theta5) -cos(theta5)  0 0;
0              0           0 1];

T56= [cos(theta6) -sin(theta6) 0 0;
0                0         1 0;
-sin(theta6) -cos(theta6)  0 0;
0              0           0 1];

%% Wrist and tool
T6W= [1  0  0  0;
0  -1  0  0;
0  0  -1 -80;
0  0  0  1];

TB0= [1 0  0  0;
0 -1 0  0;
0 0  -1  400;
0 0  0  1];

%% Placement of End Effector
TBTtarget=eulerZYX2T(563.879,-139.427,-34.414,100.551*pi/180,29.536*pi/180,178.188*pi/180); %location of end effector. // Location from robotdk
TWT=eulerZYX2T(556.600,-168.144,-86.590,100.551*pi/180,29.536*pi/180,178.188*pi/180);% the tool with respect the base  // FK works with Robodk
%% Changing the target
T06=inv(TB0)*TBTtarget;  %Remove the base so i have treansfomation 0->6

t06=[T06(1,4);
T06(2,4);
T06(3,4)];

%% step one
r_x=TBTtarget(1,3)
r_y=TBTtarget(2,3) %The rotation from T06. This is from the third collumn.
r_z=TBTtarget(3,3)

direction=[r_x,;
r_y;     %made into a roation vector.   These are the values from the third collumn
r_z];
t04=t06-(direction*-80)% Here it is as you wanted it. My d is -80. D6 is the lenght from frame 6 to the wrist right?
%% step Two
T04=T01*T12*T23*T34;    % Here i do forward kinematics, I defined the matrixes above but this gives Transformation from 0 to 4.
x=T04(1,4)               % Here i Equal x,y,z to the so I can  do this
y=T04(2,4)                %x=f(J1, J2, J3), y=f(J1, J2, J3), z=f(J2, J3)
z=T04(3,4)                  % in the next section.
%% step Three solving equations
eqn1 = x==t04(1,1);
eqn2 = z==t04(2,1);      %% Here the equations should had been solved but there are unsolveable.
eqn3 = y==t04(3,1);



Whether you can use this technique depends on the actual arrangement of your actual robot. I will give an overview of the approach given some assumptions about your robot and then if you add details of your robot e.g. diagram of the joints I can provide more detail if required.

Assumptions:

• Your robot arm has the same arrangement in the last 3 joints as the robot shown in slide 2 here: https://www.uio.no/studier/emner/matnat/ifi/INF3480/v14/undervisningsmatriale/lec08_inv_jaco.pdf (this also describes the same approach that I have written here)
• Joints are labelled J1 to J6.
• Frames are labelled F0 to F6.
• $${}^0T_1$$ is a transformation from F0 to F1 and is a function of J1, $${}^1T_2$$ is a function of J2 etc.
• You are able calculate all transformations as a function of joint angles (forward kinematics).

Now look at your robot and consider the following:

• The position of F4 e.g. the translation component xyz of $${}^0T_4$$ is not affected by changes in J4, J5 or J6. I.e. It is a function only of J1, J2 J3. (I think this is where you've misunderstood that it is the last frame that should be unaffected)

• $${}^3R_6$$ is not affected by changes in J1, J2, J3 i.e. it is a function of only J4, J5, J6.

• For a given $${}^0T_6$$ i.e. end position (that you choose), the position of F4 is fixed i.e. not affected by any joints.

Do not continue until you're confident that the above statements are correct.

So to do the IK:

1. Given a desired pose $${}^0T_6$$, you can calculate the actual numerical values for x,y,z of $${}^0T_4$$ - it is the F4-F6 link lengths $$d$$ (which are known values) back down the z axis of F6 (and J5 which is the same). The z axis of F6 is the 3rd column of the rotation matrix $${}^0R_6$$ (which is a known value, because it's whatever you set it to). $$\text{direction} = \text{3rd column of } {}^0R_6 \\ {}^0t_4 = {}^0t_6 - d * \text{direction} \\$$

Where 't' is the translation part only from the full transformation matrix 'T'. You will get a result with known numerical values.

1. Calculate FK for $${}^0T_4$$. The xyz components will only depend on J1, J2, J3. You will have something like x=f(J1, J2, J3), y=f(J1, J2, J3), z=f(J2, J3)

2. Set the results of (1) and (2) equal to each other and you now you have 3 equations (one for x, one for y, one for z).

3. Solve for J1, J2, J3. If this is what you've already done so far then great. Now you know J1, J2, J3 for a given $${}^0T_6$$.

4. Calculate $${}^0R_3$$ using forward kinematics. Since you know J1, J2, J3 you will get a matrix of numbers (no unknowns).

5. Calculate $${}^3R_6$$ using forward kinematics. Since you don't know J4, J5, J6 yet, the values in this matrix will be expressed as a function of those joints.

6. Now you have $${}^0R_3$$ with known values, $${}^0R_6$$ with known values, and $${}^3R_6$$ as a function of J4, J5, J6.

$${}^0R_3 * {}^3R_6 = {}^0R_6 \\ ({}^0R_3)^{-1} * {}^0R_3 * {}^3R_6 = ({}^0R_3)^{-1} * {}^0R_6 \\ {}^3R_6 = ({}^0R_3)^{-1} * {}^0R_6$$

You have 1 matrix of unknowns on the left, and a matrix of known values on the right i.e. you have 9 equations.

1. Solve these equations to find J4, J5, J6.

And you're done! Hopefully if you are still unsure you can narrow it down to one (or more) of the steps above.

Note on doing FK with unknown variables: If you are doing this by hand it's quite easy to make mistakes because there are a lot of terms. It is much easier to use a symbolic algebra library, they are available for matlab and python and probably other languages. Wherever something e.g. a, d, alpha etc is zero, put it as zero, it will make the results much simpler.

• I think it applies to my robot because it is the kuka kr 6 700. However I had some problems with your method. In step one where you multiply d to the rotation matrix; how should I subtract "d*direction" with T06 when the first is a 3x3 and the t06 is 4x4. Also when T04 has too many variables how should I solve that even with Matlab. Too many variables perhaps i misunderstood.Also, there is a tool for how much does that change things? I think these are my only obstacles. May 9 '20 at 16:04
• I will edit for clarity but ${}^0t_6{}$ with the lower case 't' was meant to represent just the translation part of the transformation matrix 'T' so it's just a 3d vector. 'direction' is also a vector - it's the 3rd column of the rotation matrix from of ${}^0T_6{}$. So vector = vector - scalar*vector. When you say ${}^0T_6{}$ has too many variables - how many do you get? It should only be a function of J1, J2, J3 angles (and the known link lengths). I wasn't suggesting to solve using matlab (although maybe that's possible), just to use if for all the matrix multiplication with unknowns. May 10 '20 at 6:39
• So I were able to set up what you were talking about up to the point 3. I couldn't solve them because they were unsolvable. Here is matlab code with i.imgur.com/ZlClbcU.png and here is the equation i get from that code. i.imgur.com/Cp4bbEv.png Also thanks for the help until now. Where did I go wrong? It it because this solution doesn't work with this robot? May 10 '20 at 9:13
• Your question would be improved if you put a diagram of the robot showing joints and frame positions. Regarding the unsolvable equations - after a quick look at your robot online, it should be fine with this approach. What exactly do your x,y,z from ${}^0T_4{}$ look like? I would expect something like $$h = bsin(\theta_2) + (c+d)sin(\theta_{23})\\ x = hcos(\theta_1)\\ y = hsin(\theta_1)\\ z = a+bcos(\theta_2) + (c+d)cos(\theta_{23})$$ (where $\theta_{23} = \theta_2 + \theta_3$, and a-d are link lengths) Might look different depending on joint offsets or if I've made a mistake. May 10 '20 at 13:29
• My x,y,z does not resemble this in anyway or form. The fourth column of my x,y,z axis gives me this: i.imgur.com/YHV1oFX.png Thats too long. I have put my Matlab code above with comments. I have also added joints and frame positions. However if i take atan2 of my x and y from my t04 i get the correct value for theta1. Coudlnt use it to my advantage though. Any suggestions to the code that can give me a solvable equation that gives the first three thetas? May 10 '20 at 15:23