# Solution to Inverse Kinematics of a 6 DOF Articulated robot with a wrist joint

I'm working on getting a solution to the inverse kinematics of a 6DOF Articulated robot with a wrist joint.

I derived the robot's forward kinematics firstly, using these angles $$θ_1 = 60.102$$ $$θ_2 = 65.220$$ $$θ_3 = 40.390$$ $$θ_4 = 70.215$$ $$θ_5 = 40.002$$ $$θ_6 = 30.211$$

Using these joint parameters, I found a total transformation matrix

$$a_1 = 0.18;~~a_2 = 0.6;~~a_3 = 0.12;~~a_4 = 0;~~a_5 = 0;~~a_6 = 0;$$

$$d_1 = 0.4;~~d_2 = 0;~~d_3 = 0;~~d_4 = 0.62;~~d_5 = 0;~~d_6 = 0.115;$$

$$α_1 = 90;~~α_2 = 0;~~α_3 = -90;~~α_4 = 90;~~α_5 = -90;~~α_6 = 0;$$

This is the resulting total transformation matrix:

$$-0.920890~~~0.342692~~~~0.185808~~~~-0.077297$$ $$-0.010244~~~0.455209~~~~-0.890326~~~-0.273985$$ $$-0.389689~~~-0.821795~~~-0.415687~~~0.845690$$ $$0.000000~~~~0.000000~~~~0.000000~~~~1.000000$$

Now, I'm trying to find an analytical Inverse Kinematic solution that will output the original joint angles I used for the robot's forward kinematics using the total transformation matrix I found.

I used this formula to find the first three angles by decoupling the robot. The output of $$\theta_1$$ is correct but the output of $$\theta_2$$ & $$\theta_3$$ are wrong.

$$θ_1 = atan2(y_c,x_c) + π$$ $$θ_3 = atan2(s3,c3)$$ $$θ_2 = atan2((z_c-d_1),\sqrt(x_c^2+y_c^2)) - atan2(a_3s_3,a_2+a_3c_3)$$

KEY

$$x_c = P_x - (d_6*r_{13})$$ $$y_c = P_y - (d_6*r_{23})$$ $$z_c = P_z - (d_6*r_{33})$$ $$s_3 = sine theta 3$$ $$s_3 = sqrt(1-c_3^2)$$ $$c_3 = cosine theta 3$$ $$c_3 = \frac {(x_c^2 + y_c^2 + (z_c-d_1)^2 - a_2^2 - a_3^2)}{(2*a_2*a_3)}$$ $$a_2,a_3,d_1 == robot link parameters, defined earlier$$

Does someone know of another IK analytical method I can use that will get me my original joint angles?

• The problem is that for a 6-DOF arm there is not a unique set of angles that arrive at the solution Mar 19, 2022 at 16:04
• can you post the axis assignment of your robot at the home configuration ? Oct 19, 2022 at 16:40