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?

Question

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

Thank you for reading and hopefully answering, Have a nice day.

Here is my matlab code. I have also added a picture of the lengths and the DH-table we made. https://i.stack.imgur.com/Cn62S.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); 

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

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

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

4. 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$.

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

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

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

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